motor control code by JYB
Dependencies: mbed-dev-f303 FastPWM3
main.cpp
- Committer:
- MrStark
- Date:
- 2020-12-15
- Revision:
- 48:e9f6441bacf5
- Parent:
- 47:f4ecf3e0576a
File content as of revision 48:e9f6441bacf5:
/// 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_MODE 6 // this mode is used to test motor, which allow user control motor's torque directly on the GUI software #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 #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 <string> PreferenceWriter prefs(6); GPIOStruct gpio; ControllerStruct controller; COMStruct com; ObserverStruct observer; // Serial pc(PA_2, PA_3); Serial pc(PC_6, PC_7); //SPI1_MISO SPI1_MOSI? DataPackage datapackage; CAN can(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name CANMessage rxMsg; CANMessage txMsg; DigitalOut led_debug(PB_1); PositionSensorAM5147 spi(16384, 0.0, NPP); // int CPR, float offset, int ppairs //volatile int count = 0; volatile int state = REST_MODE; volatile int state_change; volatile float test_tff,test_kp,test_kd,test_pos_des,test_vel_des; volatile float test_ia,test_ib,test_ic,test_id,test_iq; volatile float test_mech_angle,test_mech_speed; 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); } pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT); can.write(txMsg); } } void enter_menu_state(void){ pc.printf("\n\r\n\r\n\r"); pc.printf(" Commands:\n\r"); wait_us(10); pc.printf(" m - Motor Mode\n\r"); wait_us(10); pc.printf(" c - Calibrate Encoder\n\r"); wait_us(10); pc.printf(" s - Setup\n\r"); wait_us(10); pc.printf(" e - Display Encoder\n\r"); wait_us(10); pc.printf(" z - Set Zero Position\n\r"); wait_us(10); pc.printf(" t - Motor Test Mode\n\r"); wait_us(10); pc.printf(" esc - Exit to Menu\n\r"); wait_us(10); state_change = 0; gpio.enable->write(0); gpio.led->write(0); } void enter_setup_state(void){ pc.printf("\n\r\n\r Configuration Options \n\r\n\n"); wait_us(10); pc.printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value"); wait_us(10); pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW); wait_us(10); pc.printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID); wait_us(10); pc.printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER); wait_us(10); pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Torque Limit (N-m)", "0.0", "18.0", TORQUE_LIMIT); wait_us(10); pc.printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT); wait_us(10); pc.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_test_state(void){ pc.printf("\n\r\n\r Control Options \n\r\n\n"); wait_us(10); pc.printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value"); wait_us(10); pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "t", "Torque", "-18", "18", controller.t_ff); wait_us(10); pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "k", "Stiffness", "0", "500", controller.kp); wait_us(10); pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "d", "Damping", "0", "5.0", controller.kd); wait_us(10); pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "p", "Position", "-12.5", "12.5", controller.p_des); wait_us(10); pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "v", "Velocity", "-45", "45", controller.v_des); 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; pc.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 gpio.led->write(0); // Turn off status LED wait(.2); gpio.enable->write(0); // Turn off gate drive pc.printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r"); state_change = 0; } void print_encoder(void){ pc.printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition()); wait(.05); } /// Current Sampling Interrupt /// /// This runs at 40 kHz, regardless of of the mode the controller is in /// int LLLL = 0; bool led_flag = 0; bool flag_test_first = true; 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); // 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; /// LLLL ++; if(LLLL==10000){ LLLL=0; led_flag = 1-led_flag; led_debug=(led_flag); // pc.printf("in interrupt\n\r"); } /// Check state machine state, and run the appropriate function /// switch(state){ case REST_MODE: // Do nothing if(state_change){ enter_menu_state(); flag_test_first = true; } break; case CALIBRATION_MODE: // Run encoder calibration procedure if(state_change){ calibrate(); } 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"); } */ // controller.t_ff = 1; torque_control(&controller); 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 TEST_MODE: if(flag_test_first){ // enter_test_state(); // enter_torque_mode(); 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); flag_test_first = false; // Turn on status LED } else{ controller.t_ff = test_tff; controller.kp = test_kp; controller.p_des = test_pos_des; controller.kd = test_kd; controller.v_des = test_vel_des; torque_control(&controller); commutate(&controller, &observer, &gpio, controller.theta_elec); test_mech_angle = controller.theta_mech; test_mech_speed = controller.dtheta_mech; test_ia = controller.i_a; test_ib = controller.i_b; test_ic = controller.i_c; // test_ia = observer.i_q_est; // test_ib = controller.v_d; // test_ic = controller.v_q; test_id = controller.i_d; test_iq = controller.i_q; // test_iq = observer.i_q_est; // test_id = observer.i_d_est; // test_iq = controller->i_q_filt; } 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 '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); spi.Sample(DT); wait_us(20); M_OFFSET = spi.GetMechPosition(); if (!prefs.ready()) prefs.open(); prefs.flush(); // Write new prefs to flash prefs.close(); prefs.load(); spi.SetMechOffset(M_OFFSET); pc.printf("\n\r Saved new zero position: %.4f\n\r\n\r", M_OFFSET); break; case 't': // this state is used to test motor through GUI state = TEST_MODE; state_change = 1; } } 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: pc.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; } } else if(state == TEST_MODE){ if(c == 13){ switch(cmd_id){ case 't': // controller.t_ff = fmaxf(fminf(atof(cmd_val), TORQUE_LIMIT), -TORQUE_LIMIT); test_tff = fmaxf(fminf(atof(cmd_val), TORQUE_LIMIT), -TORQUE_LIMIT); break; case 'k': // controller.kp = fmaxf(fminf(atof(cmd_val), KP_MAX), KP_MIN); test_kp = fmaxf(fminf(atof(cmd_val), KP_MAX), KP_MIN); break; case 'd': // controller.kd = fmaxf(fminf(atof(cmd_val), KD_MAX), KD_MIN); test_kd = fmaxf(fminf(atof(cmd_val), KD_MAX), KD_MIN); break; case 'p': // controller.p_des = fmaxf(fminf(atof(cmd_val), P_MAX), P_MIN); test_pos_des = fmaxf(fminf(atof(cmd_val), P_MAX), P_MIN); break; case 'v': // controller.v_des = fmaxf(fminf(atof(cmd_val), V_MAX), V_MIN); test_vel_des = fmaxf(fminf(atof(cmd_val), V_MAX), V_MIN); break; default: // pc.printf("\n\r '%c' Not a valid command prefix\n\r", cmd_id); break; } 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; } char_count++; } } } } int main() { controller.v_bus = V_BUS; controller.mode = 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, 2); // 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); int lut[128] = {0}; memcpy(&lut, &ENCODER_LUT, sizeof(lut)); spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table pc.baud(921600); // set serial baud rate // pc.baud(460800); wait(.01); pc.printf("\n\r\n\r 3.3, With obs, vdff, no cogging\n\r\n\r"); wait(.01); pc.printf("\n\r Debug Info:\n\r"); pc.printf(" Firmware Version: %s\n\r", VERSION_NUM); pc.printf(" ADC1 Offset: %d ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset); pc.printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET); pc.printf(" Output Zero Position: %.4f\n\r", M_OFFSET); pc.printf(" CAN ID: %d\n\r", CAN_ID); pc.attach(&serial_interrupt); // attach serial interrupt // ============================================================================================================= // add watch dog here // Watchdog &watchdog = Watchdog::get_instance(); // ============================================================================================================= state_change = 1; datapackage.info.init_flag1 = 0xffffffff; datapackage.info.init_flag2 = 0xfffefdfc; datapackage.info.check_num = 0; while(1) { // pc.printf("hello_wooden"); // pc.printf("I's still working\n\r"); // pc.printf("adc1: %d, adc2: %d, adc3: %d",controller.adc1_raw,controller.adc2_raw,controller.adc3_raw); if(state == TEST_MODE){ // datapackage.info.I_a = controller.i_a; // datapackage.info.I_b = controller.i_b; // datapackage.info.I_c = controller.i_c; // datapackage.info.I_q = controller.i_q; // datapackage.info.I_d = controller.i_d; // datapackage.info.Angle_mech = controller.theta_mech; // datapackage.info.Angle_elec = controller.theta_elec; // datapackage.info.Control_tff = controller.t_ff; // datapackage.info.Control_kp = controller.kp; // datapackage.info.Control_p = controller.theta_mech; // datapackage.info.Control_kd = controller.kd; // datapackage.info.Control_v = controller.dtheta_mech; datapackage.info.I_a = test_ia; datapackage.info.I_b = test_ib; datapackage.info.I_c = test_ic; datapackage.info.I_q = test_iq; datapackage.info.I_d = test_id; datapackage.info.Angle_mech = test_mech_angle; datapackage.info.Angle_elec = test_mech_speed; // actually this return the speed datapackage.info.Control_tff = test_tff; datapackage.info.Control_kp = test_kp; datapackage.info.Control_p = test_pos_des; datapackage.info.Control_kd = test_kd; datapackage.info.Control_v = test_vel_des; // pc.write(datapackage.decode, 60); // string s(datapackage.decode); // pc.printf((const char *)datapackage.decode );wait_us(900); for(int i=0;i<60;i++){ pc.putc(datapackage.decode[i]); // wait_us(30); wait_us(300); } //wait_us(5000); } else{ // can.write(txMsg); wait(1.0); } } }