Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 57:7b3dec9349db, committed 2021-10-05
- Comitter:
- jianghan1913
- Date:
- Tue Oct 05 07:18:13 2021 +0000
- Parent:
- 56:fe5056ac6740
- Commit message:
- motor
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
mbed-dev.lib | Show diff for this revision Revisions of this file |
diff -r fe5056ac6740 -r 7b3dec9349db main.cpp --- a/main.cpp Thu Oct 10 15:03:12 2019 +0000 +++ b/main.cpp Tue Oct 05 07:18:13 2021 +0000 @@ -20,10 +20,10 @@ #include "mbed.h" #include "PositionSensor.h" #include "structs.h" -#include "foc.h" +#include "foc.h" #include "calibration.h" #include "hw_setup.h" -#include "math_ops.h" +#include "math_ops.h" #include "current_controller_config.h" #include "hw_config.h" #include "motor_config.h" @@ -33,7 +33,7 @@ #include "PreferenceWriter.h" #include "CAN_com.h" #include "DRV.h" - + PreferenceWriter prefs(6); GPIOStruct gpio; @@ -53,42 +53,41 @@ //DigitalOut drv_en_gate(PA_11); DRV832x drv(&drv_spi, &drv_cs); -PositionSensorAM5147 spi(16384, 0.0, NPP); +PositionSensorAM5147 spi(16384, 0.0, NPP); volatile int count = 0; volatile int state = REST_MODE; volatile int state_change; -void onMsgReceived() { +void onMsgReceived() +{ //msgAvailable = true; //printf("%d\n\r", rxMsg.id); - can.read(rxMsg); - if((rxMsg.id == CAN_ID)){ + 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))){ + 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))){ + } 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))){ + 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){ + } 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){ +void enter_menu_state(void) +{ drv.disable_gd(); - reset_foc(&controller); + reset_foc(&controller); //gpio.enable->write(0); printf("\n\r\n\r\n\r"); printf(" Commands:\n\r"); @@ -107,9 +106,10 @@ wait_us(10); state_change = 0; gpio.led->write(0); - } +} -void enter_setup_state(void){ +void enter_setup_state(void) +{ printf("\n\r\n\r Configuration Options \n\r\n\n"); wait_us(10); printf(" %-4s %-31s %-5s %-6s %-2s\n\r\n\r", "prefix", "parameter", "min", "max", "current value"); @@ -133,9 +133,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){ +} + +void enter_torque_mode(void) +{ drv.enable_gd(); //gpio.enable->write(1); controller.ovp_flag = 0; @@ -146,9 +147,10 @@ gpio.led->write(1); // Turn on status LED state_change = 0; printf("\n\r Entering Motor Mode \n\r"); - } - -void calibrate(void){ +} + +void calibrate(void) +{ drv.enable_gd(); //gpio.enable->write(1); gpio.led->write(1); // Turn on status LED @@ -161,77 +163,77 @@ //printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r"); //drv.disable_gd(); //state_change = 0; - - } - -void print_encoder(void){ + +} + +void print_encoder(void) +{ printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition()); //printf("%d\n\r", spi.GetRawPosition()); wait(.001); - } +} /// Current Sampling Interrupt /// /// This runs at 40 kHz, regardless of of the mode the controller is in /// //float testing[1000]; //float testing2[1000]; -extern "C" void TIM1_UP_TIM10_IRQHandler(void) { - if (TIM1->SR & TIM_SR_UIF ) { +extern "C" void TIM1_UP_TIM10_IRQHandler(void) +{ + if (TIM1->SR & TIM_SR_UIF ) { //gpio.led->write(1); ///Sample current always /// ADC1->CR2 |= 0x40000000; // Begin sample and conversion - //volatile int delay; + //volatile int delay; //for (delay = 0; delay < 55; delay++); - + spi.Sample(DT); // sample position sensor /* if(count < 10){printf("%d\n\r", spi.GetRawPosition());} count ++; - */ + */ controller.adc2_raw = ADC2->DR; // Read ADC Data Registers controller.adc1_raw = ADC1->DR; controller.adc3_raw = ADC3->DR; controller.theta_elec = spi.GetElecPosition(); controller.theta_mech = (1.0f/GR)*spi.GetMechPosition(); - controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity(); + 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; //filter the dc link voltage measurement - + + - /// - + /// Check state machine state, and run the appropriate function /// - switch(state){ + switch(state) { case REST_MODE: // Do nothing - if(state_change){ + if(state_change) { enter_menu_state(); - } + } update_observer(&controller, &observer); break; - + case CALIBRATION_MODE: // Run encoder calibration procedure - if(state_change){ + if(state_change) { calibrate(); - } + } break; case INIT_TEMP_MODE: - if(state_change){ + if(state_change) { enter_torque_mode(); count = 0; observer.resistance = 0.0f; - } + } controller.i_d_ref = -10.0f; controller.i_q_ref = 0.0f; - commutate(&controller, &observer, &gpio, controller.theta_elec); + commutate(&controller, &observer, &gpio, controller.theta_elec); - if(count > 200) - { + if(count > 200) { float r_meas = controller.v_d*(DTC_MAX-DTC_MIN)/(controller.i_d*SQRT3); //testing2[count-100] = controller.i_d; observer.resistance += .001f*r_meas; } - if(count > 1200) - { + if(count > 1200) { count = 0; state = REST_MODE; state_change = 1; @@ -239,9 +241,8 @@ observer.temperature = (double)(T_AMBIENT + ((observer.resistance/R_NOMINAL) - 1.0f)*254.5f); printf("Winding Resistance: %f\n\r", observer.resistance); printf("Winding Temperature: %f\n\r", observer.temperature); - - if(R_NOMINAL==0) - { + + if(R_NOMINAL==0) { printf("Saving winding resistance\n\r"); R_NOMINAL = observer.resistance; if (!prefs.ready()) prefs.open(); @@ -250,65 +251,63 @@ } //for(int i = 0; i<1000; i++){printf("%f \n\r", testing[i]);} } - - count++; + + count++; break; case MOTOR_MODE: // Run torque control - if(state_change){ + 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. + ->write(0); + controller.ovp_flag = 1; + state = REST_MODE; + state_change = 1; + printf("OVP Triggered!\n\r"); + } + */ + + 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; } - 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. - ->write(0); - controller.ovp_flag = 1; - state = REST_MODE; - state_change = 1; - printf("OVP Triggered!\n\r"); - } - */ - 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; - } - - torque_control(&controller); - update_observer(&controller, &observer); - field_weaken(&controller); - commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop - controller.timeout++; + torque_control(&controller); + update_observer(&controller, &observer); + field_weaken(&controller); + commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop + controller.timeout++; - if(controller.otw_flag) - { - state = REST_MODE; - state_change = 1; - gpio.led->write(0); + if(controller.otw_flag) { + state = REST_MODE; + state_change = 1; + gpio.led->write(0); + } + + count++; } - - count++; - } - - + + break; - + case SETUP_MODE: - if(state_change){ + if(state_change) { enter_setup_state(); } break; case ENCODER_MODE: print_encoder(); break; - } - } - //gpio.led->write(0); - TIM1->SR = 0x0; // reset the status register + } + } + //gpio.led->write(0); + TIM1->SR = 0x0; // reset the status register } @@ -318,19 +317,22 @@ /// Manage state machine with commands from serial terminal or configurator gui /// /// Called when data received over serial /// -void serial_interrupt(void){ - while(pc.readable()){ +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){ + 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; @@ -353,19 +355,18 @@ wait_us(20); M_OFFSET = spi.GetMechPosition(); if (!prefs.ready()) prefs.open(); - prefs.flush(); // Write new prefs to flash - prefs.close(); - prefs.load(); + prefs.flush(); // Write new prefs to flash + prefs.close(); + prefs.load(); spi.SetMechOffset(M_OFFSET); printf("\n\r Saved new zero position: %.4f\n\r\n\r", M_OFFSET); - + break; - } - - } - else if(state == SETUP_MODE){ - if(c == 13){ - switch (cmd_id){ + } + + } 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; @@ -393,52 +394,53 @@ 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(); + prefs.close(); + prefs.load(); state_change = 1; char_count = 0; cmd_id = 0; - for(int i = 0; i<8; i++){cmd_val[i] = 0;} + for(int i = 0; i<8; i++) { + cmd_val[i] = 0; } - else{ - if(char_count == 0){cmd_id = c;} - else{ + } 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){ + } else if (state == ENCODER_MODE) { + switch (c) { case 27: state = REST_MODE; state_change = 1; break; - } } - else if (state == MOTOR_MODE){ - switch (c){ + } else if (state == MOTOR_MODE) { + switch (c) { case 'd': controller.i_q_ref = 0; controller.i_d_ref = 0; - } } - } + } - -int main() { +} + +int main() +{ controller.v_bus = V_BUS; controller.mode = 0; Init_All_HW(&gpio); // Setup PWM, ADC, GPIO wait_us(100); - + gpio.enable->write(1); wait_us(100); drv.calibrate(); @@ -447,58 +449,83 @@ wait_us(100); drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x1, 0x1, 0x1, SEN_LVL_1_0); // calibrate shunt amplifiers wait_us(100); - zero_current(&controller.adc1_offset, &controller.adc2_offset); + zero_current(&controller.adc1_offset, &controller.adc2_offset); wait_us(100); drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x1, 0x0, 0x0, 0x0, SEN_LVL_1_0); wait_us(100); drv.write_OCPCR(TRETRY_50US, DEADTIME_50NS, OCP_NONE, OCP_DEG_8US, VDS_LVL_1_88); - + //drv.enable_gd(); drv.disable_gd(); //zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset //drv.enable_gd(); wait_us(100); - + reset_foc(&controller); // Reset current controller reset_observer(&observer); // Reset observer //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt - + wait_us(100); NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 2); // commutation > communication NVIC_SetPriority(CAN1_RX0_IRQn, 3); - // attach 'CAN receive-complete' interrupt handler - - // If preferences haven't been user configured yet, set defaults + // attach 'CAN receive-complete' interrupt handler + + // If preferences haven't been user configured yet, set defaults prefs.load(); // Read flash - can.filter(CAN_ID , 0xFFF, CANStandard, 0); + can.filter(CAN_ID, 0xFFF, CANStandard, 0); txMsg.id = CAN_MASTER; txMsg.len = 6; rxMsg.len = 8; - can.attach(&onMsgReceived); - - if(isnan(E_OFFSET)){E_OFFSET = 0.0f;} - if(isnan(M_OFFSET)){M_OFFSET = 0.0f;} - if(isnan(I_BW) || I_BW==-1){I_BW = 1000;} - if(isnan(I_MAX) || I_MAX ==-1){I_MAX=40;} - if(isnan(I_FW_MAX) || I_FW_MAX ==-1){I_FW_MAX=12;} - if(isnan(CAN_ID) || CAN_ID==-1){CAN_ID = 1;} - if(isnan(CAN_MASTER) || CAN_MASTER==-1){CAN_MASTER = 0;} - if(isnan(CAN_TIMEOUT) || CAN_TIMEOUT==-1){CAN_TIMEOUT = 1000;} - if(isnan(R_NOMINAL) || R_NOMINAL==-1){R_NOMINAL = 0.0f;} - if(isnan(TEMP_MAX) || TEMP_MAX==-1){TEMP_MAX = 125.0f;} - if(isnan(I_MAX_CONT) || I_MAX_CONT==-1){I_MAX_CONT = 14.0f;} + can.attach(&onMsgReceived); + + if(isnan(E_OFFSET)) { + E_OFFSET = 0.0f; + } + if(isnan(M_OFFSET)) { + M_OFFSET = 0.0f; + } + if(isnan(I_BW) || I_BW==-1) { + I_BW = 1000; + } + if(isnan(I_MAX) || I_MAX ==-1) { + I_MAX=40; + } + if(isnan(I_FW_MAX) || I_FW_MAX ==-1) { + I_FW_MAX=12; + } + if(isnan(CAN_ID) || CAN_ID==-1) { + CAN_ID = 1; + } + if(isnan(CAN_MASTER) || CAN_MASTER==-1) { + CAN_MASTER = 0; + } + if(isnan(CAN_TIMEOUT) || CAN_TIMEOUT==-1) { + CAN_TIMEOUT = 1000; + } + if(isnan(R_NOMINAL) || R_NOMINAL==-1) { + R_NOMINAL = 0.0f; + } + if(isnan(TEMP_MAX) || TEMP_MAX==-1) { + TEMP_MAX = 125.0f; + } + if(isnan(I_MAX_CONT) || I_MAX_CONT==-1) { + I_MAX_CONT = 14.0f; + } spi.SetElecOffset(E_OFFSET); // Set position sensor offset spi.SetMechOffset(M_OFFSET); spi.Sample(1.0f); - if(spi.GetMechPosition() > PI){spi.SetMechOffset(M_OFFSET+2.0f*PI);} // now zeroes to +- 30 degrees about nominal, independent of rollover point - else if (spi.GetMechPosition() < -PI){spi.SetMechOffset(M_OFFSET-2.0f*PI);} - + if(spi.GetMechPosition() > PI) { + spi.SetMechOffset(M_OFFSET+2.0f*PI); // now zeroes to +- 30 degrees about nominal, independent of rollover point + } else if (spi.GetMechPosition() < -PI) { + spi.SetMechOffset(M_OFFSET-2.0f*PI); + } + int lut[128] = {0}; memcpy(&lut, &ENCODER_LUT, sizeof(lut)); spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table init_controller_params(&controller); - + pc.baud(921600); // set serial baud rate wait(.01); @@ -510,11 +537,11 @@ printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET); printf(" Output Zero Position: %.4f\n\r", M_OFFSET); printf(" CAN ID: %d\n\r", CAN_ID); - + TIM1->CR1 ^= TIM_CR1_UDIS; - + pc.attach(&serial_interrupt); // attach serial interrupt @@ -523,18 +550,20 @@ //drv.print_faults(); wait(.1); //printf("%.3f %.3f\n\r" , observer.temperature, observer.q_in); - if(controller.otw_flag){gpio.led->write(!gpio.led->read());} - /* + if(controller.otw_flag) { + gpio.led->write(!gpio.led->read()); + } + /* if(state == MOTOR_MODE) { - if(controller.otw_flag){gpio.led->write(!gpio.led->read());} - //printf("%f %f\n\r", controller.dtheta_mech, controller.i_d_ref); - //printf("%.3f %.3f %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance); - //printf("%.3f %.3f %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.dtheta_elec); - //printf("%.3f %.3f %.3f %.3f\n\r", controller.dtheta_elec, observer.resistance, observer.temperature, observer.temp_measured); - //printf("%.3f %.3f\n\r" , observer.temperature, observer.temp_measured); + if(controller.otw_flag){gpio.led->write(!gpio.led->read());} + //printf("%f %f\n\r", controller.dtheta_mech, controller.i_d_ref); + //printf("%.3f %.3f %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance); + //printf("%.3f %.3f %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.dtheta_elec); + //printf("%.3f %.3f %.3f %.3f\n\r", controller.dtheta_elec, observer.resistance, observer.temperature, observer.temp_measured); + //printf("%.3f %.3f\n\r" , observer.temperature, observer.temp_measured); } - + */ }
diff -r fe5056ac6740 -r 7b3dec9349db mbed-dev.lib --- a/mbed-dev.lib Thu Oct 10 15:03:12 2019 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://os.mbed.com/users/benkatz/code/mbed-dev-f303/#36facd806e4a