Shao Rui
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);
}