1
Dependencies: mbed-dev-f303 FastPWM3
Diff: main.cpp
- Revision:
- 49:7eac11914980
- Parent:
- 48:1b51771c3647
- Child:
- 50:1fe5c2c53af1
--- a/main.cpp Fri Feb 07 11:31:37 2020 +0000 +++ b/main.cpp Thu Mar 19 03:48:24 2020 +0000 @@ -8,7 +8,7 @@ #define MOTOR_MODE 2 #define SETUP_MODE 4 #define ENCODER_MODE 5 -#define JOINT_CALIBRATION_MODE 6 +#define TEST_TRAJECTORY_MODE 6 #define J_CALIBRATION_MODE 7 #define VERSION_NUM "1.6" @@ -17,7 +17,7 @@ //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 test_jointround_flag=0; int stop_sign=0; #include "mbed.h" #include "PositionSensor.h" @@ -60,6 +60,8 @@ volatile int count = 0; volatile int state = REST_MODE; volatile int state_change; +volatile float Joint_init =0; //Joint intial angle +volatile int J_M_flag = 0; // Joint motor angle combine void onMsgReceived() { //msgAvailable = true; @@ -170,11 +172,32 @@ 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 + + //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 + + /*************同时设置转子和关节零位置同步****************///暂时不需要,标定结束,按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); +*/ + /*************同时设置转子和关节零位置同步****************/ + 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"); @@ -189,24 +212,7 @@ 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******************************/ @@ -217,7 +223,11 @@ //enter_torque_mode(); count = 0; printf("test\n\r"); - + controller.p_des=0; + controller.v_des = 1.5f; + controller.kp = 0; + controller.kd = 5.0f; + controller.t_ff = 0; /* if((1.0f/GR)* spi.GetMechPosition()<=(2*PI)) { @@ -241,8 +251,14 @@ void print_encoder(void){ - printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition()); - wait(.05); + printf(" AS5147Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition()); + printf(" MA700:Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", ma700.GetMechPosition(), ma700.GetElecPosition(), ma700.GetRawPosition()); + printf(" Joint Angle: %f Mech1: %f Mech: %f\n\r", controller.theta_joint_raw*57.2957795, -controller.theta_mech1*57.2957795*GR, controller.theta_mech*57.2957795*GR); + + // 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+controller.theta_mech)*2807.49319614; + wait(.5); } /// Current Sampling Interrupt /// @@ -260,7 +276,7 @@ spi.Sample(DT); ma700.Sample(DT); // sample position sensor controller.theta_elec = spi.GetElecPosition(); - controller.theta_mech = (1.0f/GR)*spi.GetMechPosition(); + // 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; @@ -268,9 +284,56 @@ //////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.theta_mech1 = (1.0f/GR)*ma700.GetMechPosition(); + controller.dtheta_mech1 =(1.0f/GR)*ma700.GetMechVelocity(); controller.dtheta_elec1 = ma700.GetElecVelocity(); + + ///hjb add joint angle + controller.theta_joint_raw= -(ma700.GetMechPosition()+spi.GetMechPosition()); + if(controller.dtheta_mech>0) {controller.theta_joint_raw += PI/180.0f;} //compensate + else if(controller.dtheta_mech<0) {controller.theta_joint_raw -= PI/180.0f;} + if(controller.theta_joint_raw<0){controller.theta_joint_raw += 2.0f*PI;} + if(controller.theta_joint_raw>=360){controller.theta_joint_raw -= 2.0f*PI;} + + controller.theta_joint_raw_fil =0.99f*controller.theta_joint_raw_pre + 0.01f*controller.theta_joint_raw ;//filter + controller.theta_joint_raw_pre =controller.theta_joint_raw_fil; + + //"rad 0~2pi" ("deg" =-(controller.theta_mech1+controller.theta_mech)*2807.49319614; + //int Ncycle=0; + //float Ncycle_mod = 0.0f; + //float Mech_mod = 0.0f; + controller.Ncycle = controller.theta_joint_raw/(2.0f*PI/GR); + controller.Ncycle_mod = fmod(controller.theta_joint_raw,2.0f*PI/GR); + controller.Mech_mod = fmod(spi.GetMechPosition(),2.0f*PI); + if(abs(controller.Ncycle_mod*57.2957795-controller.Mech_mod*57.2957795/GR)>4) //not in the error range + { + if (controller.Mech_mod>PI) // + { + controller.Ncycle -= 1; + } + else if(controller.Mech_mod<PI) // + { + controller.Ncycle += 1; + } + + } + controller.theta_joint= controller.Ncycle*(2.0f*PI/GR)+controller.Mech_mod/GR; //In the real use, should turn to the theta_mech + if(J_M_flag == 0) + { + if(0<abs((controller.theta_joint- controller.theta_joint_raw)*57.2957795)<3) + { + Joint_init = controller.theta_joint- (1.0f/GR)*spi.GetMechPosition(); + controller.theta_mech = controller.theta_joint;////controller.theta_joint; // easy way to use, whether available in the shock? + J_M_flag = 1; + } + controller.theta_mech = controller.theta_joint_raw; + } + else if(J_M_flag == 1) + { + controller.theta_mech = Joint_init + (1.0f/GR)*spi.GetMechPosition(); + } + + /////shaorui end////////////////// /* controller.c++; @@ -304,9 +367,10 @@ } break; - case JOINT_CALIBRATION_MODE: // Run encoder calibration procedure + case TEST_TRAJECTORY_MODE: // Run encoder calibration procedure if(state_change){ - joint_flag=1; + test_jointround_flag=1; + J_M_flag = 0; stop_sign=0; jointcalibrate(); @@ -343,8 +407,9 @@ commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop controller.timeout += 1; + + count++; /* - count++; if(count == 4000){ printf("%.4f\n\r", controller.dtheta_mech); count = 0; @@ -393,7 +458,7 @@ break; case 't': - state = JOINT_CALIBRATION_MODE; + state = TEST_TRAJECTORY_MODE; state_change = 1; break; case 'j': @@ -428,8 +493,8 @@ 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 ); + printf("\n\r Saved new M-J J-J zero position: %.4f %.4f\n\r\n\r", M_OFFSET, JOINT_M_OFFSET); + /* for(int i=0;i<300;i++) { printf("%.3d %.3d\n\r",i,__int_reg[i] ); @@ -438,7 +503,7 @@ { printf("%.3d %.3f\n\r",j,__float_reg[j] ); } - + */ break; } @@ -530,15 +595,19 @@ if(isnan(M_OFFSET)){M_OFFSET = 0.0f;} spi.SetElecOffset(E_OFFSET); // Set position sensor offset spi.SetMechOffset(M_OFFSET); + if(isnan(JOINT_E_OFFSET)){JOINT_E_OFFSET = 0.0f;} + if(isnan(JOINT_M_OFFSET)){JOINT_M_OFFSET = 0.0f;} + ma700.SetElecOffset(JOINT_E_OFFSET); // Set position sensor offset + ma700.SetMechOffset(JOINT_M_OFFSET); int lut[128] = {0}; - int joint[128]={0}; + int joint[128]={0}; //shaorui memcpy(&lut, &ENCODER_LUT, sizeof(lut)); spi.WriteLUT(lut); memcpy(&joint, &ENCODER_JOINT , sizeof(joint)); spi.WriteLUT(joint); - pc.baud(115200);//pc.baud(921600); // set serial baud rate + pc.baud(460800);//pc.baud(921600); // set serial baud rate wait(.01); - pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r"); + pc.printf("\n\r\n\r QXSLAB Joint\n\r\n\r"); wait(.01); printf("\n\r Debug Info:\n\r"); printf(" Firmware Version: %s\n\r", VERSION_NUM); @@ -546,19 +615,20 @@ printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET); printf(" Output Zero Position: %.4f\n\r", M_OFFSET); printf(" CAN ID: %d\n\r", CAN_ID); - + + controller.theta_joint_raw_pre=-(ma700.GetMechPosition()+spi.GetMechPosition()); //hjb added, for joint encoder filter pc.attach(&serial_interrupt); // attach serial interrupt - + J_M_flag = 0; //state_change = 1; while(1) { - wait(.1); + // wait(.1); if(state == MOTOR_MODE) { - if(joint_flag==1) + if(test_jointround_flag==1) { - if((1.0f/GR)* spi.GetMechPosition()<=0.01) + if(controller.theta_joint_raw*57.2957795<=1) { //if(stop_sign==0) //{ @@ -574,13 +644,13 @@ /* else { - joint_flag=0; + test_jointround_flag=0; controller.v_des =0; } */ } - else if((1.0f/GR)* spi.GetMechPosition()>=(2*PI)) + else if(controller.theta_joint_raw*57.2957795>=(359)) { //stop_sign=1; controller.v_des = 0; @@ -595,9 +665,9 @@ } } - wait(.1); + //wait(.1); // printf("%.3f\n\r",(1.0f/GR)* spi.GetMechPosition()); - // printf("%.3d, %.3d\n\r",joint_flag, stop_sign); + // printf("%.3d, %.3d\n\r",test_jointround_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)); @@ -608,15 +678,21 @@ // 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 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+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); - + //printf("m:%.3f\n\r,j:%.3f\n\r",m_position,j_position); + if(count >= 4000){ + //printf("J: %.3f Mec: %.3f Jerr: %.3f JVerr: %.3f\n\r",controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, (controller.p_des - controller.theta_mech)*57.2957795,(controller.v_des - controller.dtheta_mech)*57.2957795); + printf("Pdes: %.3f Vdes: %.3f Kp: %.3f Kd: %.3f Tff: %.3f\n\r",controller.p_des*57.2957795, controller.v_des*57.2957795, controller.kp,controller.kd,controller.t_ff); + printf("Prel: %.3f Vrel: %.3f T: %.3f \n\r",controller.theta_mech*57.2957795, controller.dtheta_mech*57.2957795, controller.i_q_filt*KT_OUT); + count = 0; + } + - i++; - wait(.5); + i++; + //wait(.5); }