Shao Rui
1
Dependencies: mbed-dev-f303 FastPWM3
Diff: main.cpp
- Revision:
- 52:d4d5e3414865
- Parent:
- 51:29e1686e8b3e
- Child:
- 53:32218a36df05
--- a/main.cpp Thu Aug 06 07:25:51 2020 +0000
+++ b/main.cpp Sat Aug 08 13:43:20 2020 +0000
@@ -251,9 +251,9 @@
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(" AS5147Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition()*57.2957795, spi.GetElecPosition()*57.2957795, spi.GetRawPosition());
+ printf(" MA700:Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", ma700.GetMechPosition()*57.2957795, ma700.GetElecPosition()*57.2957795, ma700.GetRawPosition());
+ printf(" Joint raw: %f Joint: %f Mech: %f\n\r", controller.theta_joint_raw*57.2957795, controller.theta_joint*57.2957795, 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;
@@ -270,11 +270,13 @@
ADC1->CR2 |= 0x40000000; // Begin sample and conversion
//volatile int delay;
//for (delay = 0; delay < 55; delay++);
+ spi.Sample(DT);
+ ma700.Sample(DT);
+
controller.adc2_raw = ADC2->DR; // Read ADC Data Registers
controller.adc1_raw = ADC1->DR;
controller.adc3_raw = ADC3->DR;
- spi.Sample(DT);
- ma700.Sample(DT); // sample position sensor
+ // sample position sensor
controller.theta_elec = spi.GetElecPosition();
// controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();
@@ -293,7 +295,10 @@
//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;}
+ else if(controller.theta_joint_raw>=2.0f*PI){controller.theta_joint_raw -= 2.0f*PI;}
+
+ if(controller.theta_joint_raw<0){controller.theta_joint_raw += 2.0f*PI;} //may be still below 0
+ else if(controller.theta_joint_raw>=2.0f*PI){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;
@@ -305,21 +310,13 @@
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;
- }
-
- }
- */
+
+ if(controller.Mech_mod<0){controller.Mech_mod += 2.0f*PI;}
+ else if(controller.Mech_mod>=2.0f*PI){controller.Mech_mod -= 2.0f*PI;}
+
controller.theta_joint= controller.Ncycle*(2.0f*PI/GR)+controller.Mech_mod/GR; //In the real use, should turn to the theta_mech
+ if(controller.theta_joint<0){controller.theta_joint += 2.0f*PI;}
+ else if(controller.theta_joint>=2.0f*PI){controller.theta_joint -= 2.0f*PI;}
if(J_M_flag == 0)
{
if(0<abs(controller.Ncycle_mod*57.2957795-controller.Mech_mod*57.2957795/GR)<2)
@@ -589,36 +586,60 @@
NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 0); // commutation > communication
NVIC_SetPriority(CAN1_RX0_IRQn, 3);
- can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
-
+ // If preferences haven't been user configured yet, set defaults
+ prefs.load(); // Read flash
+ can.filter(CAN_ID , 0xFFF, CANStandard, 0);
txMsg.id = CAN_MASTER;
txMsg.len = 6;
rxMsg.len = 8;
- can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler
- prefs.load(); // Read flash
+ can.attach(&onMsgReceived);
+
if(isnan(E_OFFSET)){E_OFFSET = 0.0f;}
if(isnan(M_OFFSET)){M_OFFSET = 0.0f;}
spi.SetElecOffset(E_OFFSET); // Set position sensor offset
spi.SetMechOffset(M_OFFSET);
+
+ pc.baud(460800);//pc.baud(921600);
+ //=====benk 2020 last version===//
+
+ spi.Sample(1.0f);
+ printf("MP: %.3f\n\r",spi.GetMechPosition());
+ if(spi.GetMechPosition() > 2.0f*PI){spi.SetMechOffset(M_OFFSET+2.0f*PI);} // now zeroes to +- 30 degrees about nominal, independent of rollover point
+ else if (spi.GetMechPosition() < 0){spi.SetMechOffset(M_OFFSET-2.0f*PI);}
+ //=====benk 2020 last version===//
+
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);
+ //=====benk 2020 last version===//
+ ma700.Sample(1.0f);
+ printf("JP: %.3f\n\r",ma700.GetMechPosition());
+ if(ma700.GetMechPosition() > 2.0f*PI){ma700.SetMechOffset(JOINT_M_OFFSET+2.0f*PI);} // now zeroes to +- 30 degrees about nominal, independent of rollover point
+ else if (ma700.GetMechPosition() < 0){ma700.SetMechOffset(JOINT_M_OFFSET-2.0f*PI);}
+
+ spi.Sample(1.0f);
+ ma700.Sample(1.0f);
+ printf("MP: %.3f JP: %.3f\n\r",spi.GetMechPosition(),ma700.GetMechPosition());
+
+ //=====benk 2020 last version===//
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
+ // set serial baud rate
wait(.01);
+ printf("MP: %.3f JP: %.3f\n\r",spi.GetMechPosition(),ma700.GetMechPosition());
+
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);
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(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET*57.2957795);
+ printf(" Output Zero Position: %.4f\n\r", M_OFFSET*57.2957795);
printf(" CAN ID: %d\n\r", CAN_ID);
controller.theta_joint_raw_pre=-(ma700.GetMechPosition()+spi.GetMechPosition()); //hjb added, for joint encoder filter
@@ -689,7 +710,9 @@
// 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("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("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;