Shao Rui
1
Dependencies: mbed-dev-f303 FastPWM3
Revision 48:1b51771c3647, committed 2020-02-07
- Comitter:
- shaorui
- Date:
- Fri Feb 07 11:31:37 2020 +0000
- Parent:
- 47:55bdc4d5096b
- Commit message:
- test;
Changed in this revision
diff -r 55bdc4d5096b -r 1b51771c3647 CAN/CAN_com.cpp
--- 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");
diff -r 55bdc4d5096b -r 1b51771c3647 CAN/CAN_com.h --- 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
diff -r 55bdc4d5096b -r 1b51771c3647 Calibration/calibration.cpp
--- 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
diff -r 55bdc4d5096b -r 1b51771c3647 Calibration/calibration.h --- 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
diff -r 55bdc4d5096b -r 1b51771c3647 Config/motor_config.h --- 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.
diff -r 55bdc4d5096b -r 1b51771c3647 Config/user_config.h --- 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[];
diff -r 55bdc4d5096b -r 1b51771c3647 FOC/foc.cpp
--- 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));
diff -r 55bdc4d5096b -r 1b51771c3647 Joint_Calibration/joint_calibration.cpp
--- /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
diff -r 55bdc4d5096b -r 1b51771c3647 Joint_Calibration/joint_calibration.h --- /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
diff -r 55bdc4d5096b -r 1b51771c3647 MA700Sensor/MA700Sensor.cpp
--- /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
diff -r 55bdc4d5096b -r 1b51771c3647 MA700Sensor/MA700Sensor.h
--- /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
diff -r 55bdc4d5096b -r 1b51771c3647 PositionSensor/PositionSensor.cpp
--- 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
diff -r 55bdc4d5096b -r 1b51771c3647 PositionSensor/PositionSensor.h
--- 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);
diff -r 55bdc4d5096b -r 1b51771c3647 PreferenceWriter/PrefrenceWriter.cpp
--- 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;
diff -r 55bdc4d5096b -r 1b51771c3647 main.cpp
--- 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);
+
+ }
+
+
}
}
diff -r 55bdc4d5096b -r 1b51771c3647 structs.h
--- 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{