1

Dependencies:   mbed-dev-f303 FastPWM3

Files at this revision

API Documentation at this revision

Comitter:
shaorui
Date:
Fri Feb 07 11:31:37 2020 +0000
Parent:
47:55bdc4d5096b
Commit message:
test;

Changed in this revision

CAN/CAN_com.cpp Show annotated file Show diff for this revision Revisions of this file
CAN/CAN_com.h Show annotated file Show diff for this revision Revisions of this file
Calibration/calibration.cpp Show annotated file Show diff for this revision Revisions of this file
Calibration/calibration.h Show annotated file Show diff for this revision Revisions of this file
Config/motor_config.h Show annotated file Show diff for this revision Revisions of this file
Config/user_config.h Show annotated file Show diff for this revision Revisions of this file
FOC/foc.cpp Show annotated file Show diff for this revision Revisions of this file
Joint_Calibration/joint_calibration.cpp Show annotated file Show diff for this revision Revisions of this file
Joint_Calibration/joint_calibration.h Show annotated file Show diff for this revision Revisions of this file
MA700Sensor/MA700Sensor.cpp Show annotated file Show diff for this revision Revisions of this file
MA700Sensor/MA700Sensor.h Show annotated file Show diff for this revision Revisions of this file
PositionSensor/PositionSensor.cpp Show annotated file Show diff for this revision Revisions of this file
PositionSensor/PositionSensor.h Show annotated file Show diff for this revision Revisions of this file
PreferenceWriter/PrefrenceWriter.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
structs.h Show annotated file Show diff for this revision Revisions of this file
--- a/CAN/CAN_com.cpp	Sat Dec 07 08:01:06 2019 +0000
+++ b/CAN/CAN_com.cpp	Fri Feb 07 11:31:37 2020 +0000
@@ -41,20 +41,21 @@
 /// 3: [velocity[3-0], kp[11-8]]
 /// 4: [kp[7-0]]
 /// 5: [kd[11-4]]
-/// 6: [kd[3-0], torque[11-8]]
-/// 7: [torque[7-0]]
+/// 6: [kd[3-0], ETX,ETY校验值,ETX=1时设置为0,ETY=1时设置为1,ETX=1,ETY=1时设置为2]
+/// 7: [BCT校验值[7-0]]
 void unpack_cmd(CANMessage msg, ControllerStruct * controller){
         int p_int = (msg.data[0]<<8)|msg.data[1];
         int v_int = (msg.data[2]<<4)|(msg.data[3]>>4);
         int kp_int = ((msg.data[3]&0xF)<<8)|msg.data[4];
         int kd_int = (msg.data[5]<<4)|(msg.data[6]>>4);
-        int t_int = ((msg.data[6]&0xF)<<8)|msg.data[7];
+     controller->sidebct = ((msg.data[6]&0xF)<<8)|msg.data[7];//设置为传送BCT校验值,以及设置ETX,ETY的值
+        //int t_int = ((msg.data[6]&0xF)<<8)|msg.data[7];//设置为传送BCT校验值,以及设置ETX,ETY的值
         
         controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16);
         controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 12);
         controller->kp = uint_to_float(kp_int, KP_MIN, KP_MAX, 12);
         controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 12);
-        controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12);
+        //controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12);
     //printf("Received   ");
     //printf("%.3f  %.3f  %.3f  %.3f  %.3f   %.3f", controller->p_des, controller->v_des, controller->kp, controller->kd, controller->t_ff, controller->i_q_ref);
     //printf("\n\r");
--- a/CAN/CAN_com.h	Sat Dec 07 08:01:06 2019 +0000
+++ b/CAN/CAN_com.h	Fri Feb 07 11:31:37 2020 +0000
@@ -8,6 +8,8 @@
 
  #define P_MIN -12.5f
  #define P_MAX 12.5f
+ //#define P_MIN -25.0f
+ //#define P_MAX 25.0f
  #define V_MIN -45.0f
  #define V_MAX 45.0f
  #define KP_MIN 0.0f
@@ -19,6 +21,6 @@
 
 void pack_reply(CANMessage *msg, float p, float v, float t);
 void unpack_cmd(CANMessage msg, ControllerStruct * controller);
-
+extern int sidebct;
 
 #endif
\ No newline at end of file
--- a/Calibration/calibration.cpp	Sat Dec 07 08:01:06 2019 +0000
+++ b/Calibration/calibration.cpp	Fri Feb 07 11:31:37 2020 +0000
@@ -16,7 +16,8 @@
     printf("\n\r Checking phase ordering\n\r");
     float theta_ref = 0;
     float theta_actual = 0;
-    float v_d = .15f;                                                             //Put all volts on the D-Axis
+    //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;
@@ -69,8 +70,8 @@
     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
@@ -88,7 +89,8 @@
     int raw_b[n] = {0};
     float theta_ref = 0;
     float theta_actual = 0;
-    float v_d = .15f;                                                             // Put volts on the D-Axis
+    //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;
@@ -137,7 +139,7 @@
        theta_actual = ps->GetMechPositionFixed();
        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]);
+        printf("ref %.4f\n\r  actual %.4f\n\r   raw %d\n\r", theta_ref/(NPP), theta_actual, raw_f[i]);
        //theta_ref += delta;
         }
     
@@ -162,7 +164,7 @@
        theta_actual = ps->GetMechPositionFixed();                                    // 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]);
+       printf("ref %.4f\n\r  actual %.4f\n\r   raw %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i]);
        //theta_ref -= delta;
         }    
         
@@ -230,4 +232,643 @@
         prefs->close();
 
 
-    }
\ No newline at end of file
+    }
+    
+ /* 
+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();
+
+}*/
\ No newline at end of file
--- a/Calibration/calibration.h	Sat Dec 07 08:01:06 2019 +0000
+++ b/Calibration/calibration.h	Fri Feb 07 11:31:37 2020 +0000
@@ -6,8 +6,10 @@
 #include "PositionSensor.h"
 #include "PreferenceWriter.h"
 #include "user_config.h"
-
+#include "MA700Sensor.h"
 
 void order_phases(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs);
 void calibrate(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs);
+//void j_calibrate(PositionSensorMA700 *jps,PositionSensorAM5147 *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs);
+void j_calibrate(PositionSensorMA700 *jps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs);
 #endif
--- a/Config/motor_config.h	Sat Dec 07 08:01:06 2019 +0000
+++ b/Config/motor_config.h	Fri Feb 07 11:31:37 2020 +0000
@@ -1,13 +1,17 @@
 #ifndef MOTOR_CONFIG_H
 #define MOTOR_CONFIG_H
 
-#define R_PHASE 0.105f          //Ohms
-#define L_D 0.00003f            //Henries
-#define L_Q 0.00003f            //Henries
-#define KT .075f                 //N-m per peak phase amp, = WB*NPP*3/2
+//#define R_PHASE 0.105f          //Ohms
+#define R_PHASE 0.45f  
+//#define L_D 0.00003f            //Henries
+//#define L_Q 0.00003f            //Henries
+#define L_D 0.000186f            //Henries
+#define L_Q 0.000186f            //Henries
+#define KT .08f//.075f                 //N-m per peak phase amp, = WB*NPP*3/2
 #define NPP 21                  //Number of pole pairs
-#define GR 1.0f                 //Gear ratio
-#define KT_OUT 0.45f            //KT*GR
+//#define GR 1.0f                 //Gear ratio
+#define GR 49.0f                 //Gear ratio
+#define KT_OUT 4.0f//0.45f            //KT*GR
 #define WB 0.0024f               //Webers.  
 
 
--- a/Config/user_config.h	Sat Dec 07 08:01:06 2019 +0000
+++ b/Config/user_config.h	Fri Feb 07 11:31:37 2020 +0000
@@ -10,6 +10,8 @@
 #define TORQUE_LIMIT            __float_reg[3]                                  // Torque limit (current limit = torque_limit/(kt*gear ratio))
 #define THETA_MIN               __float_reg[4]                                  // Minimum position setpoint
 #define THETA_MAX               __float_reg[5]                                  // Maximum position setpoint
+#define JOINT_E_OFFSET          __float_reg[8]                                 // Encoder joint electrical offset
+#define JOINT_M_OFFSET          __float_reg[7]                                 // Encoder joint mechanical offset
 
 
 #define PHASE_ORDER             __int_reg[0]                                    // Phase swapping during calibration
@@ -17,8 +19,7 @@
 #define CAN_MASTER              __int_reg[2]                                    // CAN bus "master" ID
 #define CAN_TIMEOUT             __int_reg[3]                                    // CAN bus timeout period
 #define ENCODER_LUT             __int_reg[5]                                    // Encoder offset LUT - 128 elements long
-
-
+#define ENCODER_JOINT            __int_reg[135]                                    // Encoder offset JOINT - 120 elements long
 
 extern float __float_reg[];
 extern int __int_reg[];
--- a/FOC/foc.cpp	Sat Dec 07 08:01:06 2019 +0000
+++ b/FOC/foc.cpp	Fri Feb 07 11:31:37 2020 +0000
@@ -116,7 +116,7 @@
        /// PI Controller ///
        float i_d_error = controller->i_d_ref - controller->i_d;
        float i_q_error = controller->i_q_ref - controller->i_q  + cogging_current;
-       
+       //Ud和Uq公式
        float v_d_ff = 2.0f*(controller->i_d_ref*R_PHASE - controller->dtheta_elec*L_Q*controller->i_q_ref);   //feed-forward voltages
        float v_q_ff =  2.0f*(controller->i_q_ref*R_PHASE  + controller->dtheta_elec*(L_D*controller->i_d_ref + WB));
        
--- /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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Joint_Calibration/joint_calibration.h	Fri Feb 07 11:31:37 2020 +0000
@@ -0,0 +1,13 @@
+#ifndef JOINT_CALIBRATION_H
+#define JOINT_CALIBRATION_H
+
+#include "foc.h"
+#include "mbed.h"
+#include "PositionSensor.h"
+#include "PreferenceWriter.h"
+#include "user_config.h"
+#include "MA700Sensor.h"
+
+//void joint_calibrate(PositionSensorMA700 *jps,PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs);
+void joint_calibrate(PositionSensorMA700 *jps,PositionSensorAM5147 *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs);
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MA700Sensor/MA700Sensor.cpp	Fri Feb 07 11:31:37 2020 +0000
@@ -0,0 +1,342 @@
+
+#include "mbed.h"
+#include "MA700Sensor.h"
+#include "math_ops.h"
+#include "CAN_com.h"
+//#include "offset_lut.h"
+//#include <math.h>
+
+PositionSensorMA700::PositionSensorMA700(int CPR, float offset, int ppairs){
+    //_CPR = CPR;
+    _CPR = CPR;
+    _ppairs = ppairs;
+    JointOffset = offset;
+    rotations = 0;
+    spi = new SPI(PA_7, PA_6, PA_5);
+    spi->format(16, 1); 
+   // spi->format(16, 3);   //shaorui modify                                                      // mbed v>127 breaks 16-bit spi, so transaction is broken into 2 8-bit words
+    spi->frequency(25000000);
+    
+    cs = new DigitalOut(PA_4);
+    cs->write(1);
+    }
+    
+void PositionSensorMA700::Sample(float dt){
+    GPIOA->ODR &= ~(1 << 4);
+    //raw = spi->write(readAngleCmd);//shaorui modify
+    //raw &= 0x3FFF;   
+    raw = spi->write(0);
+    raw = raw>>1;                                                             //Extract last 14 bits
+    //raw = raw>>2;                                                             //Extract last 14 bits
+    GPIOA->ODR |= (1 << 4);
+    int off_1 = offset_lut[raw>>7];
+    int off_2 = offset_lut[((raw>>7)+1)%128];
+    int off_interp = off_1 + ((off_2 - off_1)*(raw - ((raw>>7)<<7))>>7);        // Interpolate between lookup table entries
+    int angle = raw + off_interp;      
+    //angle = 0.9*old_counts+0.1*angle;                                         // Correct for nonlinearity with lookup table from calibration
+    if(angle - old_counts > _CPR/2){
+        rotations -= 1;
+       // printf("old%d new%d\n\r",old_counts,angle);
+        }
+    else if (angle - old_counts < -_CPR/2){
+        rotations += 1;
+        // printf("old%d new%d\n\r",old_counts,angle);
+        }
+    
+    old_counts = angle;
+    oldModPosition = modPosition;
+    modPosition = ((2.0f*PI * ((float) angle))/ (float)_CPR);
+    position = (2.0f*PI * ((float) angle+(_CPR*rotations)))/ (float)_CPR;
+    MechPosition = position - MechOffset;
+    float elec = ((2.0f*PI/(float)_CPR) * (float) ((_ppairs*angle)%_CPR)) + ElecOffset;
+    if(elec < 0) elec += 2.0f*PI;
+    else if(elec > 2.0f*PI) elec -= 2.0f*PI ; 
+    ElecPosition = elec;
+    
+    float vel;
+    //if(modPosition<.1f && oldModPosition>6.1f){
+
+    if((modPosition-oldModPosition) < -3.0f){
+        vel = (modPosition - oldModPosition + 2.0f*PI)/dt;
+        }
+    //else if(modPosition>6.1f && oldModPosition<0.1f){
+    else if((modPosition - oldModPosition) > 3.0f){
+        vel = (modPosition - oldModPosition - 2.0f*PI)/dt;
+        }
+    else{
+        vel = (modPosition-oldModPosition)/dt;
+    }    
+    
+    int n = 40;
+    float sum = vel;
+    for (int i = 1; i < (n); i++){
+        velVec[n - i] = velVec[n-i-1];
+        sum += velVec[n-i];
+        }
+    velVec[0] = vel;
+    MechVelocity =  sum/((float)n);
+    ElecVelocity = MechVelocity*_ppairs;
+    ElecVelocityFilt = 0.99f*ElecVelocityFilt + 0.01f*ElecVelocity;
+    }
+
+int PositionSensorMA700::GetRawPosition(){
+    return raw;
+    }
+
+float PositionSensorMA700::GetMechPositionFixed(){
+    return MechPosition+MechOffset;
+    }
+    
+float PositionSensorMA700::GetMechPosition(){
+    return MechPosition;
+    }
+
+float PositionSensorMA700::GetElecPosition(){
+    return ElecPosition;
+    }
+
+float PositionSensorMA700::GetElecVelocity(){
+    return ElecVelocity;
+    }
+
+float PositionSensorMA700::GetMechVelocity(){
+    return MechVelocity;
+    }
+
+void PositionSensorMA700::ZeroPosition(){
+    rotations = 0;
+    MechOffset = 0;
+    Sample(.00025f);
+    MechOffset = GetMechPosition();
+    }
+    
+void PositionSensorMA700:: SetElecOffset(float offset){
+    ElecOffset = offset;
+    }
+void PositionSensorMA700::SetMechOffset(float offset){
+    MechOffset = offset;
+    }
+    
+void  PositionSensorMA700::WriteRegister( ControllerStruct * controller){
+    BCT=0x2300|(controller->sidebct&0xff);
+    BCTREAD=0x1300;
+    int judge=(controller->sidebct&0xf00)>>8;
+    ETXY=0x2500|(judge<<4);
+    ETXYREAD=0x1500;
+    int ez;
+   // readAngleCmd = 0x1400;   //shaorui modify  
+    GPIOA->ODR &= ~(1 << 4); //shaorui ADD
+    spi->write( BCT); //shaorui ADD 
+    GPIOA->ODR |= (1 << 4);   //shaorui ADD
+    GPIOA->ODR &= ~(1 << 4); //shaorui ADD
+    _test=spi->write( BCTREAD); //shaorui ADD 
+    GPIOA->ODR |= (1 << 4);   //shaorui ADD
+    GPIOA->ODR &= ~(1 << 4); //shaorui ADD
+    _test1=ez=spi->write( ETXYREAD); //shaorui ADD 
+    GPIOA->ODR |= (1 << 4);   //shaorui ADD
+    ez&=0x000F;
+     GPIOA->ODR &= ~(1 << 4); //shaorui ADD
+    spi->write( ETXY|ez); //shaorui ADD                                                        //Extract last 14 bits
+    GPIOA->ODR |= (1 << 4);   //shaorui ADD
+    
+    GPIOA->ODR &= ~(1 << 4); //shaorui ADD
+    _test2=spi->write( ETXYREAD); //shaorui ADD                                                        //Extract last 14 bits
+    GPIOA->ODR |= (1 << 4);   //shaorui ADD
+    
+    
+    
+    
+    }
+
+int PositionSensorMA700::GetCPR(){
+    return _CPR;
+    }
+    
+void PositionSensorMA700::WriteLUT(int new_lut[128]){
+    memcpy(offset_lut, new_lut, sizeof(offset_lut));
+    }
+    
+void PositionSensorMA700::ReadLUT(void){
+    for(int z=0;z<128;z++)
+    {
+        printf("lut data:%d\n\r",offset_lut[z]);}
+    }  
+
+int PositionSensorMA700::Gettest() {
+    return _test;}
+int PositionSensorMA700::Gettest1() {
+    return _test1;}
+int PositionSensorMA700::Gettest2() {
+    return _test2;}
+
+    
+/*
+
+PositionSensorEncoder::PositionSensorEncoder(int CPR, float offset, int ppairs) {
+    _ppairs = ppairs;
+    _CPR = CPR;
+    _offset = offset;
+    MechPosition = 0;
+    out_old = 0;
+    oldVel = 0;
+    raw = 0;
+    
+    // Enable clock for GPIOA
+    __GPIOA_CLK_ENABLE(); //equivalent from hal_rcc.h
+ 
+    GPIOA->MODER   |= GPIO_MODER_MODER6_1 | GPIO_MODER_MODER7_1 ;           //PA6 & PA7 as Alternate Function   !< GPIO port mode register,               Address offset: 0x00      
+    GPIOA->OTYPER  |= GPIO_OTYPER_OT_6 | GPIO_OTYPER_OT_7 ;                 //PA6 & PA7 as Inputs              !< GPIO port output type register,        Address offset: 0x04     
+    GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR6 | GPIO_OSPEEDER_OSPEEDR7 ;     //Low speed                         !< GPIO port output speed register,       Address offset: 0x08      
+    GPIOA->PUPDR   |= GPIO_PUPDR_PUPDR6_1 | GPIO_PUPDR_PUPDR7_1 ;           //Pull Down                        !< GPIO port pull-up/pull-down register,  Address offset: 0x0C      
+    GPIOA->AFR[0]  |= 0x22000000 ;                                          //AF02 for PA6 & PA7                !< GPIO alternate function registers,     Address offset: 0x20-0x24 
+    GPIOA->AFR[1]  |= 0x00000000 ;                                          //nibbles here refer to gpio8..15  !< GPIO alternate function registers,     Address offset: 0x20-0x24 
+   
+    // configure TIM3 as Encoder input
+    // Enable clock for TIM3
+    __TIM3_CLK_ENABLE();
+ 
+    TIM3->CR1   = 0x0001;                                                   // CEN(Counter ENable)='1'     < TIM control register 1
+    TIM3->SMCR  = TIM_ENCODERMODE_TI12;                                     // SMS='011' (Encoder mode 3)  < TIM slave mode control register
+    TIM3->CCMR1 = 0x1111;                                                   // CC1S='01' CC2S='01'         < TIM capture/compare mode register 1, maximum digital filtering
+    TIM3->CCMR2 = 0x0000;                                                   //                             < TIM capture/compare mode register 2
+    TIM3->CCER  = 0x0011;                                                   // CC1P CC2P                   < TIM capture/compare enable register
+    TIM3->PSC   = 0x0000;                                                   // Prescaler = (0+1)           < TIM prescaler
+    TIM3->ARR   = CPR;                                                      // IM auto-reload register
+  
+    TIM3->CNT = 0x000;  //reset the counter before we use it  
+    
+    // Extra Timer for velocity measurement
+    
+    __TIM2_CLK_ENABLE();
+    TIM3->CR2 = 0x030;                                                      //MMS = 101
+    
+    TIM2->PSC = 0x03;
+    //TIM2->CR2 |= TIM_CR2_TI1S;
+    TIM2->SMCR = 0x24;                                                      //TS = 010 for ITR2, SMS = 100 (reset counter at edge)
+    TIM2->CCMR1 = 0x3;                                                      // CC1S = 11, IC1 mapped on TRC
+    
+    //TIM2->CR2 |= TIM_CR2_TI1S;
+    TIM2->CCER |= TIM_CCER_CC1P;
+    //TIM2->CCER |= TIM_CCER_CC1NP;
+    TIM2->CCER |= TIM_CCER_CC1E;
+    
+    
+    TIM2->CR1 = 0x01;                                                       //CEN,  enable timer
+    
+    TIM3->CR1   = 0x01;                                                     // CEN
+    ZPulse = new InterruptIn(PC_4);
+    ZSense = new DigitalIn(PC_4);
+    //ZPulse = new InterruptIn(PB_0);
+    //ZSense = new DigitalIn(PB_0);
+    ZPulse->enable_irq();
+    ZPulse->rise(this, &PositionSensorEncoder::ZeroEncoderCount);
+    //ZPulse->fall(this, &PositionSensorEncoder::ZeroEncoderCountDown);
+    ZPulse->mode(PullDown);
+    flag = 0;
+
+    
+    //ZTest = new DigitalOut(PC_2);
+    //ZTest->write(1);
+    }
+    
+void PositionSensorEncoder::Sample(float dt){
+    
+    }
+
+ 
+float PositionSensorEncoder::GetMechPosition() {                            //returns rotor angle in radians.
+    int raw = TIM3->CNT;
+    float unsigned_mech = (6.28318530718f/(float)_CPR) * (float) ((raw)%_CPR);
+    return (float) unsigned_mech;// + 6.28318530718f* (float) rotations;
+}
+
+float PositionSensorEncoder::GetElecPosition() {                            //returns rotor electrical angle in radians.
+    int raw = TIM3->CNT;
+    float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*raw)%_CPR)) - _offset;
+    if(elec < 0) elec += 6.28318530718f;
+    return elec;
+}
+
+
+    
+float PositionSensorEncoder::GetMechVelocity(){
+
+    float out = 0;
+    float rawPeriod = TIM2->CCR1; //Clock Ticks
+    int currentTime = TIM2->CNT;
+    if(currentTime > 2000000){rawPeriod = currentTime;}
+    float  dir = -2.0f*(float)(((TIM3->CR1)>>4)&1)+1.0f;    // +/- 1
+    float meas = dir*180000000.0f*(6.28318530718f/(float)_CPR)/rawPeriod; 
+    if(isinf(meas)){ meas = 1;}
+    out = meas;
+    //if(meas == oldVel){
+     //   out = .9f*out_old;
+     //   }
+    
+ 
+    oldVel = meas;
+    out_old = out;
+    int n = 16;
+    float sum = out;
+    for (int i = 1; i < (n); i++){
+        velVec[n - i] = velVec[n-i-1];
+        sum += velVec[n-i];
+        }
+    velVec[0] = out;
+    return sum/(float)n;
+    }
+    
+float PositionSensorEncoder::GetElecVelocity(){
+    return _ppairs*GetMechVelocity();
+    }
+    
+void PositionSensorEncoder::ZeroEncoderCount(void){
+    if (ZSense->read() == 1 & flag == 0){
+        if (ZSense->read() == 1){
+            GPIOC->ODR ^= (1 << 4);   
+            TIM3->CNT = 0x000;
+            //state = !state;
+            //ZTest->write(state);
+            GPIOC->ODR ^= (1 << 4);
+            //flag = 1;
+        }
+        }
+    }
+
+void PositionSensorEncoder::ZeroPosition(void){
+    
+    }
+    
+void PositionSensorEncoder::ZeroEncoderCountDown(void){
+    if (ZSense->read() == 0){
+        if (ZSense->read() == 0){
+            GPIOC->ODR ^= (1 << 4);
+            flag = 0;
+            float dir = -2.0f*(float)(((TIM3->CR1)>>4)&1)+1.0f;
+            if(dir != dir){
+                dir = dir;
+                rotations +=  dir;
+                }
+
+            GPIOC->ODR ^= (1 << 4);
+
+        }
+        }
+    }
+void PositionSensorEncoder::SetElecOffset(float offset){
+    
+    }
+    
+int PositionSensorEncoder::GetRawPosition(void){
+    return 0;
+    }
+    
+int PositionSensorEncoder::GetCPR(){
+    return _CPR;
+    }
+    
+
+void PositionSensorEncoder::WriteLUT(int new_lut[128]){
+    memcpy(offset_lut, new_lut, sizeof(offset_lut));
+    }
+*/
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MA700Sensor/MA700Sensor.h	Fri Feb 07 11:31:37 2020 +0000
@@ -0,0 +1,88 @@
+#ifndef MA700SENSOR_H
+#define MA700SENSOR_H
+#include "structs.h"
+class PositionSensor1 {
+public:
+    virtual void Sample(float dt) = 0;
+    virtual float GetMechPosition() {return 0.0f;}
+    virtual float GetMechPositionFixed() {return 0.0f;}
+    virtual float GetElecPosition() {return 0.0f;}
+    virtual float GetMechVelocity() {return 0.0f;}
+    virtual float GetElecVelocity() {return 0.0f;}
+    virtual void ZeroPosition(void) = 0;
+    virtual int GetRawPosition(void) = 0;
+    virtual void SetElecOffset(float offset) = 0;
+    virtual int GetCPR(void) = 0;
+    virtual  void WriteLUT(int new_lut[128]) = 0;
+    virtual int Gettest() {return 0;}
+    virtual int Gettest1() {return 0;}
+    virtual int Gettest2() {return 0;}
+    virtual void ReadLUT(void)=0;
+    //virtual void WriteRegister( ControllerStruct * controller)=0;
+};
+  
+ /* 
+class PositionSensorEncoder: public PositionSensor {
+public:
+    PositionSensorEncoder(int CPR, float offset, int ppairs);
+    virtual void Sample(float dt);
+    virtual float GetMechPosition();
+    virtual float GetElecPosition();
+    virtual float GetMechVelocity();
+    virtual float GetElecVelocity();
+    virtual void ZeroPosition(void);
+    virtual void SetElecOffset(float offset);
+    virtual int GetRawPosition(void);
+    virtual int GetCPR(void);
+    virtual  void WriteLUT(int new_lut[128]); 
+private:
+    InterruptIn *ZPulse;
+    DigitalIn *ZSense;
+    //DigitalOut *ZTest;
+    virtual void ZeroEncoderCount(void);
+    virtual void ZeroEncoderCountDown(void);
+    int _CPR, flag, rotations, _ppairs, raw;
+    //int state;
+    float _offset, MechPosition, MechOffset, dir, test_pos, oldVel, out_old, velVec[40];
+    int offset_lut[128];
+};
+*/
+
+class PositionSensorMA700: public PositionSensor1{
+public:
+    PositionSensorMA700(int CPR, float offset, int ppairs);
+    virtual void Sample(float dt);
+    virtual float GetMechPosition();
+    virtual float GetMechPositionFixed();
+    virtual float GetElecPosition();
+    virtual float GetMechVelocity();
+    virtual float GetElecVelocity();
+    virtual int GetRawPosition();
+    virtual void ZeroPosition();
+    virtual void SetElecOffset(float offset);
+    virtual void SetMechOffset(float offset);
+    virtual int GetCPR(void);
+    virtual void WriteLUT(int new_lut[128]);
+    virtual int Gettest();
+    virtual int Gettest1(); 
+    virtual int Gettest2(); 
+    virtual void WriteRegister( ControllerStruct * controller);
+    virtual void ReadLUT(void);
+private:
+    float position, ElecPosition,JointOffset , ElecOffset, MechPosition, MechOffset, modPosition, oldModPosition, oldVel, velVec[40], MechVelocity, ElecVelocity, ElecVelocityFilt;
+    int raw, _CPR, rotations, old_counts, _ppairs;
+    SPI *spi;
+    DigitalOut *cs;
+    int readAngleCmd;
+    int BCT;
+    int ETXY;
+    int BCTREAD;
+    int ETXYREAD;
+    int _test;
+    int _test1;
+    int _test2;
+    int offset_lut[128];
+   
+};
+
+#endif
\ No newline at end of file
--- a/PositionSensor/PositionSensor.cpp	Sat Dec 07 08:01:06 2019 +0000
+++ b/PositionSensor/PositionSensor.cpp	Fri Feb 07 11:31:37 2020 +0000
@@ -128,7 +128,7 @@
     }
     
 
-
+/*
 PositionSensorEncoder::PositionSensorEncoder(int CPR, float offset, int ppairs) {
     _ppairs = ppairs;
     _CPR = CPR;
@@ -141,12 +141,12 @@
     // Enable clock for GPIOA
     __GPIOA_CLK_ENABLE(); //equivalent from hal_rcc.h
  
-    GPIOA->MODER   |= GPIO_MODER_MODER6_1 | GPIO_MODER_MODER7_1 ;           //PA6 & PA7 as Alternate Function   /*!< GPIO port mode register,               Address offset: 0x00      */
-    GPIOA->OTYPER  |= GPIO_OTYPER_OT_6 | GPIO_OTYPER_OT_7 ;                 //PA6 & PA7 as Inputs               /*!< GPIO port output type register,        Address offset: 0x04      */
-    GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR6 | GPIO_OSPEEDER_OSPEEDR7 ;     //Low speed                         /*!< GPIO port output speed register,       Address offset: 0x08      */
-    GPIOA->PUPDR   |= GPIO_PUPDR_PUPDR6_1 | GPIO_PUPDR_PUPDR7_1 ;           //Pull Down                         /*!< GPIO port pull-up/pull-down register,  Address offset: 0x0C      */
-    GPIOA->AFR[0]  |= 0x22000000 ;                                          //AF02 for PA6 & PA7                /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
-    GPIOA->AFR[1]  |= 0x00000000 ;                                          //nibbles here refer to gpio8..15   /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
+    GPIOA->MODER   |= GPIO_MODER_MODER6_1 | GPIO_MODER_MODER7_1 ;           //PA6 & PA7 as Alternate Function   //!< GPIO port mode register,               Address offset: 0x00      //
+    GPIOA->OTYPER  |= GPIO_OTYPER_OT_6 | GPIO_OTYPER_OT_7 ;                 //PA6 & PA7 as Inputs               //!< GPIO port output type register,        Address offset: 0x04      //
+    GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR6 | GPIO_OSPEEDER_OSPEEDR7 ;     //Low speed                         //!< GPIO port output speed register,       Address offset: 0x08      //
+    GPIOA->PUPDR   |= GPIO_PUPDR_PUPDR6_1 | GPIO_PUPDR_PUPDR7_1 ;           //Pull Down                         //!< GPIO port pull-up/pull-down register,  Address offset: 0x0C      //
+    GPIOA->AFR[0]  |= 0x22000000 ;                                          //AF02 for PA6 & PA7                //!< GPIO alternate function registers,     Address offset: 0x20-0x24 //
+    GPIOA->AFR[1]  |= 0x00000000 ;                                          //nibbles here refer to gpio8..15   //!< GPIO alternate function registers,     Address offset: 0x20-0x24 //
    
     // configure TIM3 as Encoder input
     // Enable clock for TIM3
@@ -296,3 +296,4 @@
 void PositionSensorEncoder::WriteLUT(int new_lut[128]){
     memcpy(offset_lut, new_lut, sizeof(offset_lut));
     }
+*/
\ No newline at end of file
--- a/PositionSensor/PositionSensor.h	Sat Dec 07 08:01:06 2019 +0000
+++ b/PositionSensor/PositionSensor.h	Fri Feb 07 11:31:37 2020 +0000
@@ -15,7 +15,7 @@
     virtual  void WriteLUT(int new_lut[128]) = 0;
 };
   
-  
+ /*
 class PositionSensorEncoder: public PositionSensor {
 public:
     PositionSensorEncoder(int CPR, float offset, int ppairs);
@@ -40,7 +40,7 @@
     float _offset, MechPosition, MechOffset, dir, test_pos, oldVel, out_old, velVec[40];
     int offset_lut[128];
 };
-
+*/
 class PositionSensorAM5147: public PositionSensor{
 public:
     PositionSensorAM5147(int CPR, float offset, int ppairs);
--- a/PreferenceWriter/PrefrenceWriter.cpp	Sat Dec 07 08:01:06 2019 +0000
+++ b/PreferenceWriter/PrefrenceWriter.cpp	Fri Feb 07 11:31:37 2020 +0000
@@ -25,7 +25,7 @@
 void PreferenceWriter::write(float x, int index) {
     __float_reg[index] = x;
 }
-
+/*
 void PreferenceWriter::flush() {
     int offs;
     for (offs = 0; offs < 256; offs++) {
@@ -36,7 +36,18 @@
     }
     __ready = false;
 }
-
+*/
+void PreferenceWriter::flush() {
+    int offs;
+    for (offs = 0; offs < 300; offs++) {
+        writer->write(offs, __int_reg[offs]);
+    }
+    for (; offs < 364; offs++) {
+        writer->write(offs, __float_reg[offs - 300]);
+    }
+    __ready = false;
+}
+/*
 void PreferenceWriter::load() {
     int offs;
     for (offs = 0; offs < 256; offs++) {
@@ -46,6 +57,17 @@
         __float_reg[offs - 256] = flashReadFloat(__sector, offs);
     }
 }
+*/
+
+void PreferenceWriter::load() {
+    int offs;
+    for (offs = 0; offs < 300; offs++) {
+        __int_reg[offs] = flashReadInt(__sector, offs);
+    }
+    for(; offs < 364; offs++) {
+        __float_reg[offs - 300] = flashReadFloat(__sector, offs);
+    }
+}
 
 void PreferenceWriter::close() {
     __ready = false;
--- a/main.cpp	Sat Dec 07 08:01:06 2019 +0000
+++ b/main.cpp	Fri Feb 07 11:31:37 2020 +0000
@@ -8,13 +8,17 @@
 #define MOTOR_MODE 2
 #define SETUP_MODE 4
 #define ENCODER_MODE 5
-
+#define JOINT_CALIBRATION_MODE 6
+#define J_CALIBRATION_MODE 7
 #define VERSION_NUM "1.6"
 
 
 float __float_reg[64];                                                          // Floats stored in flash
-int __int_reg[256];                                                             // Ints stored in flash.  Includes position sensor calibration lookup table
-
+//int __int_reg[256];                                                             // Ints stored in flash.  Includes position sensor calibration lookup table
+int __int_reg[300];  
+int test1;
+int joint_flag=0;
+int stop_sign=0;
 #include "mbed.h"
 #include "PositionSensor.h"
 #include "structs.h"
@@ -30,9 +34,12 @@
 #include "user_config.h"
 #include "PreferenceWriter.h"
 #include "CAN_com.h"
+#include "math.h"
+#include "MA700Sensor.h"
+#include "joint_calibration.h"
+PreferenceWriter prefs(6);
+//PreferenceWriter prefs(7);
 
- 
-PreferenceWriter prefs(6);
 
 GPIOStruct gpio;
 ControllerStruct controller;
@@ -40,14 +47,15 @@
 ObserverStruct observer;
 Serial pc(PA_2, PA_3);
 
-
 CAN          can(PB_8, PB_9, 1000000);      // CAN Rx pin name, CAN Tx pin name, 1000kbps
 CANMessage   rxMsg;
 CANMessage   txMsg;
 
-
-
+int i=1;//shaorui add
+float wucha=0;
+float wucha1=0;
 PositionSensorAM5147 spi(16384, 0.0, NPP);    //14 bits encoder, 21 NPP
+PositionSensorMA700 ma700(16384,0.0,NPP); //shaorui add(12/10)
 
 volatile int count = 0;
 volatile int state = REST_MODE;
@@ -73,7 +81,15 @@
             }
         else if(state == MOTOR_MODE){
             unpack_cmd(rxMsg, &controller);
+            /*
+            if(controller.sidebct1!=controller.sidebct)
+            {
+             controller.sidebct1=controller.sidebct;
+             ma700.WriteRegister(&controller);
+             }
+             */
             }
+           
         pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
         can.write(txMsg);
         }
@@ -88,6 +104,10 @@
     wait_us(10);
     printf(" c - Calibrate Encoder\n\r");
     wait_us(10);
+    printf(" j - Joint Calibrate Encoder\n\r");
+    wait_us(10);
+    printf(" t - Joint test Encoder\n\r");
+    wait_us(10);
     printf(" s - Setup\n\r");
     wait_us(10);
     printf(" e - Display Encoder\n\r");
@@ -138,6 +158,8 @@
     gpio.led->write(1);                                                    // Turn on status LED
     order_phases(&spi, &gpio, &controller, &prefs);                             // Check phase ordering
     calibrate(&spi, &gpio, &controller, &prefs);                                // Perform calibration procedure
+    //j_calibrate(&ma700,&spi, &gpio, &controller, &prefs);
+    //j_calibrate(&ma700,&gpio, &controller, &prefs);     
     gpio.led->write(0);;                                                     // Turn off status LED
     wait(.2);
     gpio.enable->write(0);                                                      // Turn off gate drive
@@ -145,6 +167,79 @@
      state_change = 0;
     }
     
+ void jocalibrate(void){
+    gpio.enable->write(1);                                                      // Enable gate drive
+    gpio.led->write(1);                                                    // Turn on status LED
+    order_phases(&spi, &gpio, &controller, &prefs);                             // Check phase ordering
+    //calibrate(&spi, &gpio, &controller, &prefs);                                // Perform calibration procedure
+    //j_calibrate(&ma700,&spi, &gpio, &controller, &prefs);
+    j_calibrate(&ma700,&gpio, &controller, &prefs);     
+    gpio.led->write(0);;                                                     // Turn off status LED
+    wait(.2);
+    gpio.enable->write(0);                                                      // Turn off gate drive
+    printf("\n\r Calibration complete.  Press 'esc' to return to menu\n\r");
+     state_change = 0;
+    }   
+    
+void  jointcalibrate(void){
+    gpio.enable->write(1);                                                      // Enable gate drive
+    gpio.led->write(1);                                                    // Turn on status LED
+    //joint_calibrate (&ma700,&spi,&gpio,&controller,&prefs);                 // Perform calibration procedure                      
+    gpio.led->write(0);                                                   // Turn off status LED
+    wait(.2);
+    gpio.enable->write(0); 
+    
+    /*************同时设置转子和关节零位置同步****************/
+    spi.SetMechOffset(0);
+    ma700.SetMechOffset(0);
+    spi.Sample(DT);
+    ma700.Sample(DT);
+    wait_us(20);
+    M_OFFSET = spi.GetMechPosition();
+  JOINT_M_OFFSET   =ma700.GetMechPosition();
+    if (!prefs.ready()) prefs.open();
+    prefs.flush();                                                  // Write new prefs to flash
+    prefs.close();    
+    prefs.load(); 
+    spi.SetMechOffset(M_OFFSET);
+    ma700.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  );
+
+    /*************同时设置转子和关节零位置同步****************/
+    
+    /************Trajectory Planning******************************/
+    
+ 
+       // enter_torque_mode();
+        state=MOTOR_MODE;
+        state_change=1;
+        //enter_torque_mode();
+        count = 0;
+        printf("test\n\r");
+     
+        /*
+        if((1.0f/GR)* spi.GetMechPosition()<=(2*PI))
+        {
+        controller.p_des=0;
+        controller.v_des = 2.0f;
+        controller.kp = 0;
+        controller.kd = 5.0f;
+        controller.t_ff = 0;
+        wait(.5);
+        *
+        } 
+        
+    
+************Trajectory Planning*****************************/  
+    
+                                                         // Turn off gate drive
+    printf("\n\r Joint_Calibration complete.  Press 'esc' to return to menu\n\r");
+    //state_change = 0;
+    }  
+    
+
+    
 void print_encoder(void){
     printf(" Mechanical Angle:  %f    Electrical Angle:  %f    Raw:  %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
     wait(.05);
@@ -162,19 +257,37 @@
         controller.adc2_raw = ADC2->DR;                                         // Read ADC Data Registers
         controller.adc1_raw = ADC1->DR;
         controller.adc3_raw = ADC3->DR;
-        spi.Sample(DT);                                                           // sample position sensor
+        spi.Sample(DT);
+        ma700.Sample(DT);                                                               // sample position sensor
         controller.theta_elec = spi.GetElecPosition();
         controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
         controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();  
         controller.dtheta_elec = spi.GetElecVelocity();
         controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE;
-        ///
+        
         
+        //////shaorui add for obtaining joint real position
+        controller.theta_elec1 = ma700.GetElecPosition();
+        controller.init2=controller.theta_mech1 = ma700.GetMechPosition();
+        controller.dtheta_mech1 =ma700.GetMechVelocity();  
+        controller.dtheta_elec1 = ma700.GetElecVelocity();
+        /////shaorui end//////////////////
+        /*
+        controller.c++;
+        if(controller.c>=20000)
+        {
+        controller.cha=controller.init2-controller.init1;
+        controller.init1=controller.init2;
+        controller.c=0;
+        printf("position: %.3f  \n\r", controller.cha*360/(2.0f*PI));
+        }
+        */
         /// Check state machine state, and run the appropriate function ///
         switch(state){
             case REST_MODE:                                                     // Do nothing
                 if(state_change){
                     enter_menu_state();
+                    wucha=0 ; //shaorui add
                     }
                 break;
             
@@ -184,6 +297,23 @@
                     }
                 break;
              
+             case J_CALIBRATION_MODE:                                            // Run encoder calibration procedure
+                if(state_change){
+                     jocalibrate();
+                    
+                    }
+                break;
+             
+             case JOINT_CALIBRATION_MODE:                                            // Run encoder calibration procedure
+                if(state_change){
+                     joint_flag=1;
+                     stop_sign=0;
+                     jointcalibrate();
+                    
+                    }
+                break;
+             
+             
             case MOTOR_MODE:                                                   // Run torque control
                 if(state_change){
                     enter_torque_mode();
@@ -200,7 +330,8 @@
                     }
                     */  
 
-                torque_control(&controller);    
+                torque_control(&controller); 
+                /*   
                 if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
                     controller.i_d_ref = 0;
                     controller.i_q_ref = 0;
@@ -208,6 +339,7 @@
                     controller.kd = 0;
                     controller.t_ff = 0;
                     } 
+                    */
                 commutate(&controller, &observer, &gpio, controller.theta_elec);           // Run current loop
                 controller.timeout += 1;
                 
@@ -259,6 +391,16 @@
                     state = CALIBRATION_MODE;
                     state_change = 1;
                     break;
+                    
+                case 't':
+                    state = JOINT_CALIBRATION_MODE;
+                    state_change = 1;
+                    break;
+                 case 'j':
+                    state = J_CALIBRATION_MODE;
+                    state_change = 1;
+                    break;
+                    
                 case 'm':
                     state = MOTOR_MODE;
                     state_change = 1;
@@ -271,17 +413,31 @@
                     state = SETUP_MODE;
                     state_change = 1;
                     break;
+                    
                 case 'z':
                     spi.SetMechOffset(0);
+                    ma700.SetMechOffset(0);
                     spi.Sample(DT);
+                    ma700.Sample(DT);
                     wait_us(20);
                     M_OFFSET = spi.GetMechPosition();
+                    JOINT_M_OFFSET = ma700.GetMechPosition();
                     if (!prefs.ready()) prefs.open();
                         prefs.flush();                                                  // Write new prefs to flash
                         prefs.close();    
                         prefs.load(); 
                     spi.SetMechOffset(M_OFFSET);
+                    ma700.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<300;i++)
+                    {
+                      printf("%.3d   %.3d\n\r",i,__int_reg[i] ); 
+                    }
+                    for(int j=0;j<64;j++)
+                    {
+                    printf("%.3d   %.3f\n\r",j,__float_reg[j] ); 
+                    }
                     
                     break;
                 }
@@ -345,6 +501,7 @@
     
     controller.v_bus = V_BUS;
     controller.mode = 0;
+    controller.sidebct1=0;
     Init_All_HW(&gpio);                                                         // Setup PWM, ADC, GPIO
 
     wait(.1);
@@ -359,7 +516,7 @@
     //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt
     
     wait(.1);
-    NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 2);                                             // commutation > communication
+    NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 0);                                             // commutation > communication
     
     NVIC_SetPriority(CAN1_RX0_IRQn, 3);
     can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
@@ -368,16 +525,17 @@
     txMsg.len = 6;
     rxMsg.len = 8;
     can.attach(&onMsgReceived);                                     // attach 'CAN receive-complete' interrupt handler    
-    
     prefs.load();                                                               // Read flash
     if(isnan(E_OFFSET)){E_OFFSET = 0.0f;}
     if(isnan(M_OFFSET)){M_OFFSET = 0.0f;}
     spi.SetElecOffset(E_OFFSET);                                                // Set position sensor offset
     spi.SetMechOffset(M_OFFSET);
     int lut[128] = {0};
+    int joint[128]={0};
     memcpy(&lut, &ENCODER_LUT, sizeof(lut));
-    spi.WriteLUT(lut);                                                          // Set potision sensor nonlinearity lookup table
-    
+    spi.WriteLUT(lut);  
+    memcpy(&joint, &ENCODER_JOINT , sizeof(joint));
+    spi.WriteLUT(joint);                             
     pc.baud(115200);//pc.baud(921600);                                                            // set serial baud rate
     wait(.01);
     pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r");
@@ -391,10 +549,77 @@
         
     pc.attach(&serial_interrupt);                                               // attach serial interrupt
     
-    state_change = 1;
+    //state_change = 1;
 
     
-    while(1) {
-
+    while(1) {  
+              wait(.1); 
+              if(state == MOTOR_MODE)
+             {
+              if(joint_flag==1)
+              {
+               if((1.0f/GR)* spi.GetMechPosition()<=0.01)
+               { 
+               //if(stop_sign==0)
+               //{
+                controller.v_des = 0;
+                wait(1);
+                controller.p_des=0;
+                controller.v_des = 1.5f;
+                controller.kp = 0;
+                controller.kd = 5.0f;
+                controller.t_ff = 0;
+                wait(.5); 
+               // }
+               /*
+                else 
+                {
+                   joint_flag=0; 
+                   controller.v_des =0;
+                   
+                } 
+                */
+                }
+              else if((1.0f/GR)* spi.GetMechPosition()>=(2*PI))
+               { 
+               //stop_sign=1;
+                controller.v_des = 0;
+                wait(1);
+                controller.p_des=0;
+                controller.v_des = -1.5f;
+                controller.kp = 0;
+                controller.kd = 5.0f;
+                controller.t_ff = 0;
+                wait(.5);
+                printf("test position:%.3f\n\r",(1.0f/GR)* spi.GetMechPosition());
+                
+               }  
+             }
+           wait(.1);
+            // printf("%.3f\n\r",(1.0f/GR)* spi.GetMechPosition());
+            // printf("%.3d,  %.3d\n\r",joint_flag, stop_sign);
+           
+            //printf("BCT: %.3x   zzz: %.3x   etxy: %.3x \n\r",ma700.Gettest(),ma700.Gettest1(),ma700.Gettest2());
+           // float joint_mech_position=-(controller.theta_mech*360/(2.0f*PI)*GR+controller.theta_mech1*360/(2.0f*PI));
+           // wucha1=(controller.theta_mech-controller.theta_mech1)*360/(2.0f*PI); 
+            //wucha1=controller.theta_mech*360/(2.0f*PI)-joint_mech_position;
+            //wucha+=abs(wucha1); 
+            //printf("M: %.3f J: %.3f E: %.3f  EA: %.3f  \n\r",controller.theta_mech*360/(2.0f*PI),controller.theta_mech1*360/(2.0f*PI),wucha1,float(wucha/i)) ; 
+            // printf("M: %.3f J: %.3f E: %.3f  EA: %.3f  \n\r",controller.theta_mech*360/(2.0f*PI),joint_mech_position,wucha1,float(wucha/i)) ; 
+            //printf("m_position: %.3f\n\r",controller.theta_mech*360/(2.0f*PI)*GR);
+            //printf("j_position: %.3f\n\r",controller.theta_mech1*360/(2.0f*PI));
+            float m_position=controller.theta_mech*57.2957795;
+           // float j_position=-controller.theta_mech1*360/(2.0f*PI)-controller.theta_mech*360/(2.0f*PI)*GR;
+           float j_position=-controller.theta_mech1*57.2957795-controller.theta_mech*2807.49319614;
+          // float j_position=-controller.theta_mech1*57.2957795;
+            printf("m:%.3f\n\r,j:%.3f\n\r",m_position,j_position);
+            
+            
+            i++;
+            wait(.5);
+            
+        }
+        
+                
     }
 }
--- a/structs.h	Sat Dec 07 08:01:06 2019 +0000
+++ b/structs.h	Fri Feb 07 11:31:37 2020 +0000
@@ -22,6 +22,8 @@
     float v_bus;
     float theta_mech, theta_elec;
     float dtheta_mech, dtheta_elec, dtheta_elec_filt;
+    float theta_mech1, theta_elec1;                       //shaorui add for joint position read
+    float dtheta_mech1, dtheta_elec1, dtheta_elec_filt1;  //shaorui add for joint position read
     float i_d, i_q, i_q_filt;
     float v_d, v_q;
     float dtc_u, dtc_v, dtc_w;
@@ -35,6 +37,10 @@
     int ovp_flag;
     float p_des, v_des, kp, kd, t_ff;
     float cogging[128];
+    float angle,angle1;//shaorui add  for test
+    float init1, init2,cha;  
+    int c,sidebct; 
+    int sidebct1;
     } ControllerStruct;
 
 typedef struct{