Shao Rui
1
Dependencies: mbed-dev-f303 FastPWM3
Revision 55:489f01927025, committed 2021-05-12
- Comitter:
- shaorui
- Date:
- Wed May 12 12:41:39 2021 +0000
- Parent:
- 54:4c9415402628
- Commit message:
- 1
Changed in this revision
--- a/Calibration/calibration.cpp Thu Sep 17 07:49:27 2020 +0000
+++ b/Calibration/calibration.cpp Wed May 12 12:41:39 2021 +0000
@@ -17,7 +17,8 @@
float theta_ref = 0;
float theta_actual = 0;
//float v_d = .15f;
- float v_d = .20f; //Put all volts on the D-Axis
+ //float v_d = .20f;
+ float v_d = .40f; //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;
@@ -90,7 +91,8 @@
float theta_ref = 0;
float theta_actual = 0;
//float v_d = .15f;
- float v_d = .20f; // Put volts on the D-Axis
+ // float v_d = .20f;
+ float v_d = .40f; // 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;
@@ -494,7 +496,8 @@
float theta_joint_actual = 0;
//float v_d = .15f;
- float v_d = .18f; // Put volts on the D-Axis
+ //float v_d = .18f;
+ float v_d = .60f; // 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;
--- a/main.cpp Thu Sep 17 07:49:27 2020 +0000
+++ b/main.cpp Wed May 12 12:41:39 2021 +0000
@@ -287,7 +287,7 @@
//////shaorui add for obtaining joint real position
controller.theta_elec1 = ma700.GetElecPosition();
controller.theta_mech1 = (1.0f/GR)*ma700.GetMechPosition();
- controller.dtheta_mech1 =(1.0f/GR)*ma700.GetMechVelocity();
+ controller.dtheta_mech1 =-(ma700.GetMechVelocity()+spi.GetMechVelocity());
controller.dtheta_elec1 = ma700.GetElecVelocity();
///hjb add joint angle
@@ -329,10 +329,12 @@
}
}
controller.theta_mech = controller.theta_joint_raw;
+
}
else if(J_M_flag == 1)
{
controller.theta_mech = Joint_init + (1.0f/GR)*spi.GetMechPosition();
+
}
@@ -374,7 +376,7 @@
case TEST_TRAJECTORY_MODE: // Run encoder calibration procedure
if(state_change){
test_jointround_flag=1;
- J_M_flag = 0;
+ //J_M_flag = 0;
stop_sign=0;
jointcalibrate();
@@ -399,7 +401,7 @@
*/
torque_control(&controller);
- /*
+
if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
controller.i_d_ref = 0;
controller.i_q_ref = 0;
@@ -407,7 +409,7 @@
controller.kd = 0;
controller.t_ff = 0;
}
- */
+
commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop
controller.timeout += 1;
@@ -660,80 +662,46 @@
// wait(.1);
if(state == MOTOR_MODE)
{
- if(test_jointround_flag==1)
- {
+
+
+
if(controller.theta_joint_raw*57.2957795<=1)
{
- //if(stop_sign==0)
- //{
+
controller.v_des = 0;
wait(1);
controller.p_des=0;
- controller.v_des = 1.0f;
+ controller.v_des = 1.5f;
controller.kp = 0;
- controller.kd = 2.0f;
+ controller.kd = 5.0f;
controller.t_ff = 0;
wait(.5);
- // }
- /*
- else
- {
- test_jointround_flag=0;
- controller.v_des =0;
-
- }
- */
}
+
else if(controller.theta_joint_raw*57.2957795>=(359))
{
//stop_sign=1;
controller.v_des = 0;
wait(1);
controller.p_des=0;
- controller.v_des = -1.0f;
+ controller.v_des = -1.5f;
controller.kp = 0;
- controller.kd = 2.0f;
+ 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",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));
- // 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+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);
- 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("Jraw:%.3f J: %.3f Mec: %.3f N: %.3d Nmod: %.3f Mmod: %.3f \n\r",controller.theta_joint_raw*57.2957795,controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, controller.Ncycle,controller.Ncycle_mod*57.2957795,controller.Mech_mod*57.2957795);
-
- printf("vd:%.3f vq: %.3f iq: %.3f Velc:%.3f\n\r",controller.v_d,controller.v_q,controller.i_q_ref,controller.dtheta_elec);
-
- 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);
+ if(count >= 40000){
+ printf("J_init: %.3f Pref: %.3f preal: %.3f JM: %.3d one: %.3f two: %.3f\n\r",Joint_init*57.2957795,(1.0f/GR)*spi.GetMechPosition()*57.2957795,controller.theta_mech*57.2957795,J_M_flag ,controller.Ncycle_mod*57.2957795-controller.Mech_mod*57.2957795/GR,(controller.theta_joint- controller.theta_joint_raw)*57.2957795);
+ // printf("Pref: %.3f qian: %.3f hou: %.3f M: %.3d \n\r", (1.0f/GR)*spi.GetMechPosition()*57.2957795,controller.theta_joint_raw*57.2957795,controller.theta_mech*57.2957795,J_M_flag);
+ // printf("v: %.3f v1: %.3f \n\r",controller.dtheta_mech*57.2957795,controller.dtheta_mech1*57.2957795);
count = 0;
}
-
-
+
i++;
- //wait(.5);
-
}
-
-
- }
+ }
}
--- a/mbed-dev.lib Thu Sep 17 07:49:27 2020 +0000 +++ b/mbed-dev.lib Wed May 12 12:41:39 2021 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/benkatz/code/mbed-dev-f303/#902f8c6731d6 +https://os.mbed.com/users/benkatz/code/mbed-dev-f303/#36facd806e4a