Modified Motor Driver Firmware to include Flash + Thermal
Dependencies: FastPWM3 mbed-dev-STM-lean
main.cpp.orig
- Committer:
- adimmit
- Date:
- 22 months ago
- Revision:
- 76:4fd876d4cf2b
- Parent:
- 35:69b24894c11d
File content as of revision 76:4fd876d4cf2b:
/// 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 VERSION_NUM "1.0.1" 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" PreferenceWriter prefs(6); GPIOStruct gpio; ControllerStruct controller; COMStruct com; VelocityEstimatorStruct velocity; //using namespace CANnucleo; CAN can(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name CANMessage rxMsg; CANMessage txMsg; Serial pc(PA_2, PA_3); PositionSensorAM5147 spi(16384, 0.0, NPP); PositionSensorEncoder encoder(4096, 0, NPP); DigitalOut toggle(PA_0); volatile int count = 0; volatile int state = REST_MODE; volatile int state_change; #define P_MIN -12.5f #define P_MAX 12.5f #define V_MIN -30.0f #define V_MAX 30.0f #define KP_MIN 0.0f #define KP_MAX 500.0f #define KD_MIN 0.0f #define KD_MAX 5.0f #define T_MIN -18.0f #define T_MAX 18.0f /// CAN Reply Packet Structure /// /// 16 bit position, between -4*pi and 4*pi /// 12 bit velocity, between -30 and + 30 rad/s /// 12 bit current, between -40 and 40; /// CAN Packet is 5 8-bit words /// Formatted as follows. For each quantity, bit 0 is LSB /// 0: [position[15-8]] /// 1: [position[7-0]] /// 2: [velocity[11-4]] /// 3: [velocity[3-0], current[11-8]] /// 4: [current[7-0]] void pack_reply(CANMessage *msg, float p, float v, float i){ int p_int = float_to_uint(p, P_MIN, P_MAX, 16); int v_int = float_to_uint(v, V_MIN, V_MAX, 12); int i_int = float_to_uint(i, -I_MAX, I_MAX, 12); msg->data[0] = CAN_ID; msg->data[1] = p_int>>8; msg->data[2] = p_int&0xFF; msg->data[3] = v_int>>4; msg->data[4] = ((v_int&0xF)<<4) + (i_int>>8); msg->data[5] = i_int&0xFF; } /// CAN Command Packet Structure /// /// 16 bit position command, between -4*pi and 4*pi /// 12 bit velocity command, between -30 and + 30 rad/s /// 12 bit kp, between 0 and 500 N-m/rad /// 12 bit kd, between 0 and 100 N-m*s/rad /// 12 bit feed forward torque, between -18 and 18 N-m /// CAN Packet is 8 8-bit words /// Formatted as follows. For each quantity, bit 0 is LSB /// 0: [position[15-8]] /// 1: [position[7-0]] /// 2: [velocity[11-4]] /// 3: [velocity[3-0], kp[11-8]] /// 4: [kp[7-0]] /// 5: [kd[11-4]] /// 6: [kd[3-0], torque[11-8]] /// 7: [torque[7-0]] void unpack_cmd(CANMessage msg, ControllerStruct * controller){ int p_int = (msg.data[0]<<8)|msg.data[1]; int v_int = (msg.data[2]<<4)|(msg.data[3]>>4); int kp_int = ((msg.data[3]&0xF)<<8)|msg.data[4]; int kd_int = (msg.data[5]<<4)|(msg.data[6]>>4); int t_int = ((msg.data[6]&0xF)<<8)|msg.data[7]; controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16); controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 12); controller->kp = uint_to_float(kp_int, KP_MIN, KP_MAX, 12); controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 12); controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12); //printf("Received "); //printf("%.3f %.3f %.3f %.3f %.3f %.3f", controller->p_des, controller->v_des, controller->kp, controller->kd, controller->t_ff, controller->i_q_ref); //printf("\n\r"); } void onMsgReceived() { //msgAvailable = true; //printf("%.3f %.3f %.3f\n\r", controller.theta_mech, controller.dtheta_mech, controller.i_q); 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; GPIOC->ODR &= !(1 << 5); } 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); can.write(txMsg); } } } void enter_menu_state(void){ printf("\n\r\n\r\n\r"); printf(" Commands:\n\r"); printf(" m - Motor Mode\n\r"); printf(" c - Calibrate Encoder\n\r"); printf(" s - Setup\n\r"); printf(" e - Display Encoder\n\r"); printf(" esc - Exit to Menu\n\r"); state_change = 0; gpio.enable->write(0); } void enter_setup_state(void){ printf("\n\r\n\r Configuration Options \n\r\n\n"); printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value"); printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW); printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID); printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER); printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Torque Limit (N-m)", "0.0", "18.0", TORQUE_LIMIT); printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT); printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r"); state_change = 0; } void enter_torque_mode(void){ 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 GPIOC->ODR |= (1 << 5); // 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 GPIOC->ODR |= (1 << 5); // Turn on status LED order_phases(&spi, &gpio, &controller, &prefs); // Check phase ordering calibrate(&spi, &gpio, &controller, &prefs); // Perform calibration procedure GPIOC->ODR &= !(1 << 5); // 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 print_encoder(void){ spi.Sample(); wait(.001); 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 /// extern "C" void TIM1_UP_TIM10_IRQHandler(void) { if (TIM1->SR & TIM_SR_UIF ) { //toggle = 1; ///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 ADC1 and ADC2 Data Registers controller.adc1_raw = ADC1->DR; /// /// Check state machine state, and run the appropriate function /// //printf("%d\n\r", state); switch(state){ case REST_MODE: // Do nothing until if(state_change){ enter_menu_state(); } 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{ count++; //toggle.write(1); controller.theta_elec = spi.GetElecPosition(); controller.theta_mech = (1.0f/GR)*spi.GetMechPosition(); controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity(); //TIM1->CCR3 = 0x708*(1.0f); //TIM1->CCR1 = 0x708*(1.0f); //TIM1->CCR2 = 0x708*(1.0f); //controller.i_q_ref = controller.t_ff/KT_OUT; torque_control(&controller); if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){ controller.i_d_ref = 0; controller.i_q_ref = 0; } //controller.i_q_ref = .5; commutate(&controller, &gpio, controller.theta_elec); // Run current loop spi.Sample(); // Sample position sensor //toggle.write(0); controller.timeout += 1; if(count == 4000){ count = 0; //wait(.001); //printf(" %.5f \n\r", controller.theta_mech); } } 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; GPIOC->ODR &= !(1 << 5); 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; } } 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; Init_All_HW(&gpio); // Setup PWM, ADC, GPIO wait(.1); gpio.enable->write(1); TIM1->CCR3 = 0x708*(1.0f); // Write duty cycles TIM1->CCR2 = 0x708*(1.0f); TIM1->CCR1 = 0x708*(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(TIM5_IRQn, 2); // set interrupt priority can.frequency(1000000); // set bit rate to 1Mbps can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //can.filter(CAN_ID, 0xF, CANStandard, 0); can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler txMsg.id = CAN_MASTER; txMsg.len = 6; rxMsg.len = 8; prefs.load(); // Read flash spi.SetElecOffset(E_OFFSET); // Set position sensor 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 wait(.01); pc.printf("\n\r\n\r HobbyKing Cheetah\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(" CAN ID: %d\n\r", CAN_ID); pc.attach(&serial_interrupt); // attach serial interrupt state_change = 1; while(1) { } }