Shao Rui
1111
Dependencies: mbed-dev-f303 FastPWM3
Diff: main.cpp
- Revision:
- 51:adc72e125b15
- Parent:
- 50:1fe5c2c53af1
--- 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));
+
}