Shao Rui
1111
Dependencies: mbed-dev-f303 FastPWM3
Revision 51:adc72e125b15, committed 2020-08-06
- Comitter:
- shaorui
- Date:
- Thu Aug 06 00:55:34 2020 +0000
- Parent:
- 50:1fe5c2c53af1
- Commit message:
- 11111
Changed in this revision
diff -r 1fe5c2c53af1 -r adc72e125b15 Calibration/calibration.cpp
--- a/Calibration/calibration.cpp Tue May 26 09:24:13 2020 +0000
+++ b/Calibration/calibration.cpp Thu Aug 06 00:55:34 2020 +0000
@@ -57,7 +57,7 @@
if(theta_ref==0){theta_start = theta_actual;}
if(sample_counter > 200){
sample_counter = 0 ;
- printf("%.4f %.4f\n\r", theta_ref/(NPP), theta_actual);
+ printf("%.4f %.4f %.4d\n\r", theta_ref/(NPP), theta_actual,ps->GetRawPosition());
}
sample_counter++;
theta_ref += 0.001f;
@@ -94,7 +94,7 @@
float v_q = 0.0f;
float v_u, v_v, v_w = 0;
float dtc_u, dtc_v, dtc_w = .5f;
-
+
///Set voltage angle to zero, wait for rotor position to settle
abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
@@ -117,7 +117,8 @@
controller->i_a = -controller->i_b - controller->i_c;
dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q); //dq0 transform on currents
float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2));
- printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r");
+ //printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r");
+ printf(" Current Angle : Rotor Angle : Raw Encoder Current-Rotor \n\r\n\r");
for(int i = 0; i<n; i++){ // rotate forwards
for(int j = 0; j<n2; j++){
theta_ref += delta;
@@ -140,7 +141,8 @@
error_f[i] = theta_ref/NPP - theta_actual;
raw_f[i] = ps->GetRawPosition();
printf("%.4f %.4f %d\n\r", theta_ref/(NPP), theta_actual, raw_f[i]);
-
+ // printf("%.4f\n\r", v_d);
+ // printf("%.4f %.4f %d %.4f\n\r", theta_ref/(NPP), theta_actual, raw_f[i],theta_ref/(NPP)-theta_actual);
// printf("ref %.4f\n\r actual %.4f\n\r raw %d\n\r", theta_ref/(NPP), theta_actual, raw_f[i]);
//theta_ref += delta;
}
@@ -167,7 +169,7 @@
error_b[i] = theta_ref/NPP - theta_actual;
raw_b[i] = ps->GetRawPosition();
printf("%.4f %.4f %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i]);
-
+ //printf("%.4f %.4f %d %.4f\n\r", theta_ref/(NPP), theta_actual, raw_b[i],theta_ref/(NPP)-theta_actual);
//printf("ref %.4f\n\r actual %.4f\n\r raw %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i]);
//theta_ref -= delta;
}
diff -r 1fe5c2c53af1 -r adc72e125b15 Config/user_config.h --- a/Config/user_config.h Tue May 26 09:24:13 2020 +0000 +++ b/Config/user_config.h Thu Aug 06 00:55:34 2020 +0000 @@ -19,7 +19,7 @@ #define CAN_MASTER __int_reg[2] // CAN bus "master" ID #define CAN_TIMEOUT __int_reg[3] // CAN bus timeout period #define ENCODER_LUT __int_reg[5] // Encoder offset LUT - 128 elements long -#define ENCODER_JOINT __int_reg[135] // Encoder offset JOINT - 120 elements long +#define ENCODER_JOINT __int_reg[135] // Encoder offset JOINT - 120 elements long extern float __float_reg[]; extern int __int_reg[];
diff -r 1fe5c2c53af1 -r adc72e125b15 MA700Sensor/MA700Sensor.cpp
--- a/MA700Sensor/MA700Sensor.cpp Tue May 26 09:24:13 2020 +0000
+++ b/MA700Sensor/MA700Sensor.cpp Thu Aug 06 00:55:34 2020 +0000
@@ -118,36 +118,7 @@
MechOffset = offset;
}
-void PositionSensorMA700::WriteRegister( ControllerStruct * controller){
- BCT=0x2300|(controller->sidebct&0xff);
- BCTREAD=0x1300;
- int judge=(controller->sidebct&0xf00)>>8;
- ETXY=0x2500|(judge<<4);
- ETXYREAD=0x1500;
- int ez;
- // readAngleCmd = 0x1400; //shaorui modify
- GPIOA->ODR &= ~(1 << 4); //shaorui ADD
- spi->write( BCT); //shaorui ADD
- GPIOA->ODR |= (1 << 4); //shaorui ADD
- GPIOA->ODR &= ~(1 << 4); //shaorui ADD
- _test=spi->write( BCTREAD); //shaorui ADD
- GPIOA->ODR |= (1 << 4); //shaorui ADD
- GPIOA->ODR &= ~(1 << 4); //shaorui ADD
- _test1=ez=spi->write( ETXYREAD); //shaorui ADD
- GPIOA->ODR |= (1 << 4); //shaorui ADD
- ez&=0x000F;
- GPIOA->ODR &= ~(1 << 4); //shaorui ADD
- spi->write( ETXY|ez); //shaorui ADD //Extract last 14 bits
- GPIOA->ODR |= (1 << 4); //shaorui ADD
-
- GPIOA->ODR &= ~(1 << 4); //shaorui ADD
- _test2=spi->write( ETXYREAD); //shaorui ADD //Extract last 14 bits
- GPIOA->ODR |= (1 << 4); //shaorui ADD
-
-
-
-
- }
+
int PositionSensorMA700::GetCPR(){
return _CPR;
diff -r 1fe5c2c53af1 -r adc72e125b15 MA700Sensor/MA700Sensor.h
--- a/MA700Sensor/MA700Sensor.h Tue May 26 09:24:13 2020 +0000
+++ b/MA700Sensor/MA700Sensor.h Thu Aug 06 00:55:34 2020 +0000
@@ -66,7 +66,6 @@
virtual int Gettest();
virtual int Gettest1();
virtual int Gettest2();
- virtual void WriteRegister( ControllerStruct * controller);
virtual void ReadLUT(void);
private:
float position, ElecPosition,JointOffset , ElecOffset, MechPosition, MechOffset, modPosition, oldModPosition, oldVel, velVec[40], MechVelocity, ElecVelocity, ElecVelocityFilt;
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));
+
}