12
Dependencies: mbed-dev-f303 FastPWM3
Diff: main.cpp
- Revision:
- 50:f460323fc492
- Parent:
- 47:55bdc4d5096b
--- a/main.cpp Thu Jan 09 01:42:00 2020 +0000 +++ b/main.cpp Wed Jun 02 00:56:21 2021 +0000 @@ -13,7 +13,8 @@ float __float_reg[64]; // Floats stored in flash -int __int_reg[256]; // Ints stored in flash. Includes position sensor calibration lookup table +int __int_reg[256]; +int flag=0; // Ints stored in flash. Includes position sensor calibration lookup table #include "mbed.h" #include "PositionSensor.h" @@ -49,15 +50,16 @@ PositionSensorAM5147 spi(16384, 0.0, NPP); //14 bits encoder, 21 NPP -volatile int count = 0; volatile int state = REST_MODE; volatile int state_change; - +volatile int count = 0; +int reg_count=0; void onMsgReceived() { //msgAvailable = true; can.read(rxMsg); if((rxMsg.id == CAN_ID)){ + flag=1; controller.timeout = 0; if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) & (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFC))){ state = MOTOR_MODE; @@ -147,6 +149,10 @@ void print_encoder(void){ printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition()); + for(reg_count=0;reg_count<=263;reg_count++) + { + printf("%d %d\n\r",reg_count,__int_reg[reg_count]); + } wait(.05); } @@ -175,6 +181,7 @@ case REST_MODE: // Do nothing if(state_change){ enter_menu_state(); + flag=0;//轨迹位置停止增加 } break; @@ -188,6 +195,7 @@ if(state_change){ enter_torque_mode(); count = 0; + controller.p_init_pos= controller.theta_mech;//shaorui add for torque test } else{ /* @@ -199,7 +207,11 @@ printf("OVP Triggered!\n\r"); } */ - + if(flag==1) + { + controller.p_des =controller.p_init_pos + controller.v_des*count/(40000); + //shaorui end + } torque_control(&controller); if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){ controller.i_d_ref = 0; @@ -210,16 +222,7 @@ } commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop controller.timeout += 1; - - /* - count++; - if(count == 4000){ - printf("%.4f\n\r", controller.dtheta_mech); - count = 0; - } - */ - - + count++; //shaorui add for torque_trajectory time count } break; case SETUP_MODE: @@ -395,6 +398,22 @@ while(1) { + //if(state == MOTOR_MODE) + // { + /* + printf("p_des: %.3f p_real: %.3f E: %.3f \n\r", controller.p_des*360/(2*PI),controller.theta_mech*360/(2*PI),(controller.p_des-controller.theta_mech)*360/(2*PI)); + + printf("v_des(r/min): %.3f v_real(r/min): %.3f E(./s): %.3f \n\r", + controller.v_des*GR*60/(2*PI),controller.dtheta_mech*GR*60/(2*PI),(controller.v_des-controller.dtheta_mech)*360/(2*PI)); + printf("kp %.3f,kd:%.3f\n\r",controller.kp,controller.kd); + printf("i_q_ref: %.3f\n\r", controller.i_q_ref); + //printf("i_d: %.3f \n\r", controller.i_d);*/ + // printf("v_real(r/min): %.3f kp: %.3f kd: %.3f vdes: %.3f pdes: %.3f iq: %.3f iq_f: %.3f\n", controller.dtheta_mech*GR*60/(2*PI),controller.kp,controller.kd, controller.v_des*GR*60/(2*PI),controller.p_des,controller.i_q,controller.i_q_filt); + printf("v_real(r/min): %.3f vdes: %.3f \n\r", controller.dtheta_mech*GR*60/(2*PI),controller.v_des*GR*60/(2*PI)); +// printf("%04.3f%03.3f%01.3f%01.3f%03.3f%01.3f%01.3f\n", controller.dtheta_mech*GR*60/(2*PI),controller.kp,controller.kd, controller.v_des*GR*60/(2*PI),controller.p_des,controller.i_q,controller.i_q_filt); + printf("q: %.3f q_filt: %.3f \n\r", controller.i_q,controller.i_q_filt); + wait(1); + // } } }