1111
Dependencies: mbed-dev-f303 FastPWM3
main.cpp
- Committer:
- shaorui
- Date:
- 2020-08-06
- Revision:
- 51:adc72e125b15
- Parent:
- 50:1fe5c2c53af1
File content as of revision 51:adc72e125b15:
/// high-bandwidth 3-phase motor control, for robots /// Written by benkatz, with much inspiration from bayleyw, nkirkby, scolton, David Otten, and others /// Hardware documentation can be found at build-its.blogspot.com /// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling #define REST_MODE 0 #define CALIBRATION_MODE 1 #define MOTOR_MODE 2 #define SETUP_MODE 4 #define ENCODER_MODE 5 #define TEST_TRAJECTORY_MODE 6 #define J_CALIBRATION_MODE 7 #define VERSION_NUM "1.6" 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[300]; int test1; int test_jointround_flag=0; int stop_sign=0; int NCycle; #include "mbed.h" #include "PositionSensor.h" #include "structs.h" #include "foc.h" #include "calibration.h" #include "hw_setup.h" #include "math_ops.h" #include "current_controller_config.h" #include "hw_config.h" #include "motor_config.h" #include "stm32f4xx_flash.h" #include "FlashWriter.h" #include "user_config.h" #include "PreferenceWriter.h" #include "CAN_com.h" #include "math.h" #include "MA700Sensor.h" #include "joint_calibration.h" PreferenceWriter prefs(6); //PreferenceWriter prefs(7); GPIOStruct gpio; ControllerStruct controller; COMStruct com; ObserverStruct observer; Serial pc(PA_2, PA_3); CAN can(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name, 1000kbps CANMessage rxMsg; CANMessage txMsg; int i=1;//shaorui add float wucha=0; float wucha1=0; PositionSensorAM5147 spi(16384, 0.0, NPP); //14 bits encoder, 21 NPP PositionSensorMA700 ma700(16384,0.0,NPP); //shaorui add(12/10) 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; can.read(rxMsg); if((rxMsg.id == CAN_ID)){ 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; state_change = 1; } else 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]==0xFD))){ state = REST_MODE; state_change = 1; gpio.led->write(0);; } else 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]==0xFE))){ spi.ZeroPosition(); } else if(state == MOTOR_MODE){ unpack_cmd(rxMsg, &controller); /* if(controller.sidebct1!=controller.sidebct) { controller.sidebct1=controller.sidebct; ma700.WriteRegister(&controller); } */ } pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT); can.write(txMsg); } } void enter_menu_state(void){ printf("\n\r\n\r\n\r"); printf(" Commands:\n\r"); wait_us(10); printf(" m - Motor Mode\n\r"); wait_us(10); printf(" c - Calibrate Encoder\n\r"); wait_us(10); printf(" j - Joint Calibrate Encoder\n\r"); wait_us(10); printf(" t - Joint test Encoder\n\r"); wait_us(10); printf(" s - Setup\n\r"); wait_us(10); printf(" e - Display Encoder\n\r"); wait_us(10); printf(" z - Set Zero Position\n\r"); wait_us(10); printf(" esc - Exit to Menu\n\r"); wait_us(10); state_change = 0; gpio.enable->write(0); gpio.led->write(0); test_jointround_flag=0; } void enter_setup_state(void){ printf("\n\r\n\r Configuration Options \n\r\n\n"); wait_us(10); printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value"); wait_us(10); printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW); wait_us(10); printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID); wait_us(10); printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER); wait_us(10); printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Torque Limit (N-m)", "0.0", "18.0", TORQUE_LIMIT); wait_us(10); printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT); wait_us(10); printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r"); wait_us(10); state_change = 0; } void enter_torque_mode(void){ controller.ovp_flag = 0; gpio.enable->write(1); // Enable gate drive reset_foc(&controller); // Tesets integrators, and other control loop parameters wait(.001); controller.i_d_ref = 0; controller.i_q_ref = 0; // Current Setpoints gpio.led->write(1); // Turn on status LED state_change = 0; printf("\n\r Entering Motor Mode \n\r"); } void calibrate(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 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 wait(.2); gpio.enable->write(0); // Turn off gate drive printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r"); state_change = 0; } 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 //calibrate(&spi, &gpio, &controller, &prefs); // Perform calibration procedure //j_calibrate(&ma700,&spi, &gpio, &controller, &prefs); j_calibrate(&ma700,&gpio, &controller, &prefs); /*************同时设置转子和关节零位置同步****************///暂时不需要,标定结束,按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"); state_change = 0; } void jointcalibrate(void){ gpio.enable->write(1); // Enable gate drive gpio.led->write(1); // Turn on status LED //joint_calibrate (&ma700,&spi,&gpio,&controller,&prefs); // Perform calibration procedure gpio.led->write(0); // Turn off status LED wait(.2); gpio.enable->write(0); /************Trajectory Planning******************************/ // enter_torque_mode(); state=MOTOR_MODE; state_change=1; //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)) { controller.p_des=0; controller.v_des = 2.0f; controller.kp = 0; controller.kd = 5.0f; controller.t_ff = 0; wait(.5); * } ************Trajectory Planning*****************************/ // Turn off gate drive printf("\n\r Joint_Calibration complete. Press 'esc' to return to menu\n\r"); //state_change = 0; } 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); // 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 /// /// This runs at 40 kHz, regardless of of the mode the controller is in /// extern "C" void TIM1_UP_TIM10_IRQHandler(void) { if (TIM1->SR & TIM_SR_UIF ) { ///Sample current always /// ADC1->CR2 |= 0x40000000; // Begin sample and conversion //volatile int delay; //for (delay = 0; delay < 55; delay++); 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 controller.theta_elec = spi.GetElecPosition(); // 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; //////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_elec1 = ma700.GetElecVelocity(); ///hjb add joint angle 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; 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)>2.5) //not in the error range { if ((2*PI-0.1)>controller.Mech_mod>PI) // { controller.Ncycle -= 1; } 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(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; } else if(J_M_flag == 1) { controller.theta_mech = Joint_init + (1.0f/GR)*spi.GetMechPosition(); } /////shaorui end////////////////// /* controller.c++; if(controller.c>=20000) { controller.cha=controller.init2-controller.init1; controller.init1=controller.init2; controller.c=0; printf("position: %.3f \n\r", controller.cha*360/(2.0f*PI)); } */ /// Check state machine state, and run the appropriate function /// switch(state){ case REST_MODE: // Do nothing if(state_change){ enter_menu_state(); wucha=0 ; //shaorui add } break; case CALIBRATION_MODE: // Run encoder calibration procedure if(state_change){ calibrate(); } break; case J_CALIBRATION_MODE: // Run encoder calibration procedure if(state_change){ jocalibrate(); } break; case TEST_TRAJECTORY_MODE: // Run encoder calibration procedure if(state_change){ test_jointround_flag=1; J_M_flag = 0; stop_sign=0; jointcalibrate(); } break; case MOTOR_MODE: // Run torque control if(state_change){ enter_torque_mode(); count = 0; } else{ /* if(controller.v_bus>28.0f){ //Turn of gate drive if bus voltage is too high, to prevent FETsplosion if the bus is cut during regen gpio.enable->write(0); controller.ovp_flag = 1; state = REST_MODE; state_change = 1; printf("OVP Triggered!\n\r"); } */ 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; controller.kp = 0; controller.kd = 0; controller.t_ff = 0; } } */ 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; } */ } break; case SETUP_MODE: if(state_change){ enter_setup_state(); } break; case ENCODER_MODE: print_encoder(); break; } } TIM1->SR = 0x0; // reset the status register } char cmd_val[8] = {0}; char cmd_id = 0; char char_count = 0; /// Manage state machine with commands from serial terminal or configurator gui /// /// Called when data received over serial /// void serial_interrupt(void){ while(pc.readable()){ char c = pc.getc(); if(c == 27){ state = REST_MODE; state_change = 1; char_count = 0; cmd_id = 0; gpio.led->write(0); for(int i = 0; i<8; i++){cmd_val[i] = 0;} } if(state == REST_MODE){ switch (c){ case 'c': state = CALIBRATION_MODE; state_change = 1; break; case 't': state = TEST_TRAJECTORY_MODE; state_change = 1; break; case 'j': state = J_CALIBRATION_MODE; state_change = 1; break; case 'm': state = MOTOR_MODE; state_change = 1; break; case 'e': state = ENCODER_MODE; state_change = 1; break; case 's': state = SETUP_MODE; state_change = 1; break; case '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 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] ); } for(int j=0;j<64;j++) { printf("%.3d %.3f\n\r",j,__float_reg[j] ); } break; } } else if(state == SETUP_MODE){ if(c == 13){ switch (cmd_id){ case 'b': I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f); break; case 'i': CAN_ID = atoi(cmd_val); break; case 'm': CAN_MASTER = atoi(cmd_val); break; case 'l': TORQUE_LIMIT = fmaxf(fminf(atof(cmd_val), 18.0f), 0.0f); break; case 't': CAN_TIMEOUT = atoi(cmd_val); break; default: printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id); break; } if (!prefs.ready()) prefs.open(); prefs.flush(); // Write new prefs to flash prefs.close(); prefs.load(); state_change = 1; char_count = 0; cmd_id = 0; for(int i = 0; i<8; i++){cmd_val[i] = 0;} } else{ if(char_count == 0){cmd_id = c;} else{ cmd_val[char_count-1] = c; } pc.putc(c); char_count++; } } else if (state == ENCODER_MODE){ switch (c){ case 27: state = REST_MODE; state_change = 1; break; } } } } int main() { controller.v_bus = V_BUS; controller.mode = 0; controller.sidebct1=0; Init_All_HW(&gpio); // Setup PWM, ADC, GPIO wait(.1); gpio.enable->write(1); TIM1->CCR3 = PWM_ARR*(1.0f); // Write duty cycles TIM1->CCR2 = PWM_ARR*(1.0f); TIM1->CCR1 = PWM_ARR*(1.0f); zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset gpio.enable->write(0); reset_foc(&controller); // Reset current controller TIM1->CR1 ^= TIM_CR1_UDIS; //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt wait(.1); NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 0); // commutation > communication NVIC_SetPriority(CAN1_RX0_IRQn, 3); can.filter(CAN_ID<<21, 0xFFE00004, 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 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); 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}; //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 wait(.01); 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(" 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 pc.attach(&serial_interrupt); // attach serial interrupt J_M_flag = 0; //state_change = 1; while(1) { // 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.5f; //GR=49 controller.v_des = 0.8f; //GR=89 controller.kp = 0; 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.5f;//GR=49 controller.v_des = -0.8f; //GR=89 controller.kp = 0; 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("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)); } i++; //wait(.5); } } }