1111
Dependencies: mbed-dev-f303 FastPWM3
Diff: main.cpp
- Revision:
- 51:adc72e125b15
- Parent:
- 50:1fe5c2c53af1
diff -r 1fe5c2c53af1 -r adc72e125b15 main.cpp --- a/main.cpp Tue May 26 09:24:13 2020 +0000 +++ b/main.cpp Thu Aug 06 00:55:34 2020 +0000 @@ -19,6 +19,7 @@ int test1; int test_jointround_flag=0; int stop_sign=0; +int NCycle; #include "mbed.h" #include "PositionSensor.h" #include "structs.h" @@ -121,6 +122,8 @@ state_change = 0; gpio.enable->write(0); gpio.led->write(0); + test_jointround_flag=0; + } void enter_setup_state(void){ @@ -252,8 +255,8 @@ void print_encoder(void){ 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); + // 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; @@ -289,42 +292,49 @@ controller.dtheta_elec1 = ma700.GetElecVelocity(); ///hjb add joint angle - controller.theta_joint_raw= -(ma700.GetMechPosition()+spi.GetMechPosition()); + controller.theta_joint_raw= -(ma700.GetMechPosition()+spi.GetMechPosition())+0.0134; + /* 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); + NCycle=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(abs(controller.Ncycle_mod*57.2957795-controller.Mech_mod*57.2957795/GR)>2.5) //not in the error range { - if (controller.Mech_mod>PI) // + if ((2*PI-0.1)>controller.Mech_mod>PI) // { controller.Ncycle -= 1; } - else if(controller.Mech_mod<PI) // + else if(0.1<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; + if(abs(controller.Ncycle_mod*57.2957795-controller.Mech_mod*57.2957795/GR)<2) + { + if(0<abs((controller.theta_joint- controller.theta_joint_raw)*57.2957795)<2) + { + 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; } @@ -395,7 +405,9 @@ */ torque_control(&controller); - /* + /* + if(test_jointround_flag!=1) + { if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){ controller.i_d_ref = 0; controller.i_q_ref = 0; @@ -403,7 +415,8 @@ controller.kd = 0; controller.t_ff = 0; } - */ + } + */ commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop controller.timeout += 1; @@ -425,7 +438,7 @@ } break; case ENCODER_MODE: - print_encoder(); + print_encoder(); break; } } @@ -447,7 +460,7 @@ state_change = 1; char_count = 0; cmd_id = 0; - gpio.led->write(0);; + gpio.led->write(0); for(int i = 0; i<8; i++){cmd_val[i] = 0;} } if(state == REST_MODE){ @@ -494,7 +507,7 @@ spi.SetMechOffset(M_OFFSET); ma700.SetMechOffset(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] ); @@ -503,7 +516,7 @@ { printf("%.3d %.3f\n\r",j,__float_reg[j] ); } - */ + break; } @@ -590,7 +603,8 @@ txMsg.len = 6; rxMsg.len = 8; can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler - prefs.load(); // Read flash + 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 @@ -599,13 +613,15 @@ 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}; //shaorui memcpy(&lut, &ENCODER_LUT, sizeof(lut)); spi.WriteLUT(lut); memcpy(&joint, &ENCODER_JOINT , sizeof(joint)); spi.WriteLUT(joint); - pc.baud(460800);//pc.baud(921600); // set serial baud rate + pc.baud(115200);// + //pc.baud(921600); // set serial baud rate wait(.01); pc.printf("\n\r\n\r QXSLAB Joint\n\r\n\r"); wait(.01); @@ -614,6 +630,7 @@ printf(" ADC1 Offset: %d ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset); printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET); printf(" Output Zero Position: %.4f\n\r", M_OFFSET); + printf(" JointOutput Zero Position: %.4f\n\r", JOINT_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 @@ -635,7 +652,8 @@ controller.v_des = 0; wait(1); controller.p_des=0; - controller.v_des = 1.5f; + // controller.v_des = 1.5f; //GR=49 + controller.v_des = 0.8f; //GR=89 controller.kp = 0; controller.kd = 5.0f; controller.t_ff = 0; @@ -656,7 +674,8 @@ controller.v_des = 0; wait(1); controller.p_des=0; - controller.v_des = -1.5f; + //controller.v_des = -1.5f;//GR=49 + controller.v_des = -0.8f; //GR=89 controller.kp = 0; controller.kd = 5.0f; controller.t_ff = 0; @@ -684,10 +703,19 @@ // float j_position=-controller.theta_mech1*57.2957795; //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 Kp: %.3f Kd: %.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, controller.kp,controller.kd); + // printf("J: %.3f Mec: %.3f Jerr: %.3f JVerr: %.3f Kp: %.3f Kd: %.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, controller.kp,controller.kd); + //printf("J: %.3f Mec: %.3f J-Mec: %.3f \n\r",controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, (controller.theta_joint - controller.theta_mech)*57.2957795); + // printf("J: %.3f Mec: %.3f J-Real: %.3f J-Mec: %.3f Jinit: %.3f \n\r",controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, spi.GetMechPosition()*57.2957795/GR,(controller.theta_joint - controller.theta_mech)*57.2957795,(controller.theta_joint- (1.0f/GR)*spi.GetMechPosition())*57.2957795); + // printf("Ele-Real:%.3f \n\rJ-Des: %.3f \n\r J-Real: %.3f \n\r Des-Real: %.3f \n\r",spi.GetMechPosition()*57.2957795,spi.GetMechPosition()*57.2957795/GR, controller.theta_joint_raw*57.2957795, spi.GetMechPosition()*57.2957795/GR-controller.theta_joint_raw*57.2957795); + printf("J: %.3f Mec: %.3f J-Mec: %.3f J-Real: %.3f J-Ncycle: %.3d NCycle: %.3d mode-error: %.3f J-M-flag: %.3d J-init: %.3f \n\r",controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, + (controller.theta_joint - controller.theta_mech)*57.2957795,controller.theta_joint_raw*57.2957795,controller.Ncycle,NCycle,abs(controller.Ncycle_mod*57.2957795-controller.Mech_mod*57.2957795/GR),J_M_flag,Joint_init*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; + + printf("input: %.3f output: %.3f -: %.3f\n\r",(1.0f/GR)*spi.GetMechPosition()*57.2957795,controller.theta_joint_raw*57.2957795,((1.0f/GR)*spi.GetMechPosition()*57.2957795-controller.theta_joint_raw*57.2957795)); + }