Jiang Han
MIT Motor FOC Control
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 |
--- 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);
}
-
+
*/
}
--- 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