Shao Rui
1
Dependencies: mbed-dev-f303 FastPWM3
Diff: main.cpp
- Revision:
- 55:d614e29c60c5
- Parent:
- 54:4c9415402628
--- a/main.cpp Thu Sep 17 07:49:27 2020 +0000
+++ b/main.cpp Wed Apr 14 11:46:16 2021 +0000
@@ -60,6 +60,7 @@
volatile int count = 0;
volatile int state = REST_MODE;
volatile int state_change;
+volatile int brake_count=0;
volatile float Joint_init =0; //Joint intial angle
volatile int J_M_flag = 0; // Joint motor angle combine
@@ -119,6 +120,8 @@
printf(" esc - Exit to Menu\n\r");
wait_us(10);
state_change = 0;
+ gpio.brake->write(0); //shut up electrical brake ---set up electrical brake 0% duty cycle, relative to period
+ brake_count=0;
gpio.enable->write(0);
gpio.led->write(0);
}
@@ -144,6 +147,8 @@
}
void enter_torque_mode(void){
+ // gpio.brake->write(0.8);
+ gpio.brake->write(1);
controller.ovp_flag = 0;
gpio.enable->write(1); // Enable gate drive
reset_foc(&controller); // Tesets integrators, and other control loop parameters
@@ -156,6 +161,7 @@
}
void calibrate(void){
+ gpio.brake->write(0.8);
gpio.enable->write(1); // Enable gate drive
gpio.led->write(1); // Turn on status LED
order_phases(&spi, &gpio, &controller, &prefs); // Check phase ordering
@@ -287,7 +293,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 +335,12 @@
}
}
controller.theta_mech = controller.theta_joint_raw;
+
}
else if(J_M_flag == 1)
{
controller.theta_mech = Joint_init + (1.0f/GR)*spi.GetMechPosition();
+
}
@@ -361,6 +369,7 @@
case CALIBRATION_MODE: // Run encoder calibration procedure
if(state_change){
calibrate();
+ gpio.brake->write(0); //shut up electrical brake ---set up electrical brake 0% duty cycle, relative to period
}
break;
@@ -374,7 +383,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();
@@ -397,9 +406,14 @@
printf("OVP Triggered!\n\r");
}
*/
-
+ brake_count++;
+ if(brake_count>=3*40000)
+ {
+ //gpio.brake->write(0.125); //set up electrical brake 12.5% duty cycle, relative to period
+ gpio.brake->write(0.3);
+ }
torque_control(&controller);
- /*
+
if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
controller.i_d_ref = 0;
controller.i_q_ref = 0;
@@ -407,7 +421,7 @@
controller.kd = 0;
controller.t_ff = 0;
}
- */
+
commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop
controller.timeout += 1;
@@ -574,7 +588,7 @@
controller.mode = 0;
controller.sidebct1=0;
Init_All_HW(&gpio); // Setup PWM, ADC, GPIO
-
+ gpio.brake->period_ms(25); //set up electrical brake 250 ms period
wait(.1);
gpio.enable->write(1);
TIM1->CCR3 = PWM_ARR*(1.0f); // Write duty cycles
@@ -660,80 +674,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);
-
}
-
-
- }
+ }
}