Tony Stark
motor control code by JYB
Dependencies: mbed-dev-f303 FastPWM3
Revision 48:e9f6441bacf5, committed 2020-12-15
- Comitter:
- MrStark
- Date:
- Tue Dec 15 08:46:20 2020 +0000
- Parent:
- 47:f4ecf3e0576a
- Commit message:
- Try to publish
Changed in this revision
--- a/BLDC_V2.lib Wed May 13 09:53:27 2020 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://os.mbed.com/users/benkatz/code/Hobbyking_Cheetah_Compact/#6cc428f3431d
--- a/Config/user_config.h Wed May 13 09:53:27 2020 +0000 +++ b/Config/user_config.h Tue Dec 15 08:46:20 2020 +0000 @@ -19,7 +19,6 @@ #define ENCODER_LUT __int_reg[5] // Encoder offset LUT - 128 elements long - extern float __float_reg[]; extern int __int_reg[];
--- a/FOC/foc.cpp Wed May 13 09:53:27 2020 +0000
+++ b/FOC/foc.cpp Tue Dec 15 08:46:20 2020 +0000
@@ -120,7 +120,7 @@
//float i_q_error = controller->i_q_ref - observer->i_q_est;
float v_d_ff = 2.0f*(controller->i_d_ref*R_PHASE - controller->dtheta_elec*L_Q*controller->i_q_ref); //feed-forward voltages
- float v_q_ff = 2.0f*(controller->i_q_ref*R_PHASE + controller->dtheta_elec*(L_D*controller->i_d_ref + WB));
+ float v_q_ff = 2.0f*(controller->i_q_ref*R_PHASE + controller->dtheta_elec*(L_D*controller->i_d_ref + WB));
controller->d_int += i_d_error;
controller->q_int += i_q_error;
@@ -179,7 +179,7 @@
void torque_control(ControllerStruct *controller){
float torque_ref = controller->kp*(controller->p_des - controller->theta_mech) + controller->t_ff + controller->kd*(controller->v_des - controller->dtheta_mech);
//float torque_ref = -.1*(controller->p_des - controller->theta_mech);
- controller->i_q_ref = torque_ref/KT_OUT;
+ controller->i_q_ref = torque_ref/KT_OUT;
controller->i_d_ref = 0.0f;
}
--- a/FOC/foc.h Wed May 13 09:53:27 2020 +0000 +++ b/FOC/foc.h Tue Dec 15 08:46:20 2020 +0000 @@ -19,4 +19,5 @@ void reset_foc(ControllerStruct *controller); void commutate(ControllerStruct *controller, ObserverStruct *observer, GPIOStruct *gpio, float theta); void torque_control(ControllerStruct *controller); + #endif
--- a/main.cpp Wed May 13 09:53:27 2020 +0000
+++ b/main.cpp Tue Dec 15 08:46:20 2020 +0000
@@ -41,7 +41,7 @@
ObserverStruct observer;
// Serial pc(PA_2, PA_3);
-Serial pc(PC_6, PC_7);
+Serial pc(PC_6, PC_7); //SPI1_MISO SPI1_MOSI?
DataPackage datapackage;
@@ -51,7 +51,7 @@
DigitalOut led_debug(PB_1);
-PositionSensorAM5147 spi(16384, 0.0, NPP);
+PositionSensorAM5147 spi(16384, 0.0, NPP); // int CPR, float offset, int ppairs
//volatile int count = 0;
volatile int state = REST_MODE;
@@ -150,7 +150,7 @@
}
void enter_torque_mode(void){
- controller.ovp_flag = 0;
+ controller.ovp_flag = 0; // ???
gpio.enable->write(1); // Enable gate drive
reset_foc(&controller); // Tesets integrators, and other control loop parameters
wait(.001);
@@ -166,7 +166,7 @@
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
+ 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");
@@ -229,6 +229,7 @@
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
@@ -258,9 +259,8 @@
count = 0;
}
*/
-
-
- }
+ }
+
break;
case SETUP_MODE:
if(state_change){
@@ -323,15 +323,18 @@
/// 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;}
- }
+ 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':
@@ -367,9 +370,9 @@
// 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){
@@ -391,7 +394,7 @@
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
@@ -401,7 +404,7 @@
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{
@@ -412,14 +415,16 @@
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){
@@ -446,24 +451,25 @@
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;}
+ if(char_count == 0){
+ cmd_id = c;
+ }
else{
cmd_val[char_count-1] = c;
-
}
- // pc.putc(c);
char_count++;
- }
- }
- }
+ }
+ }
}
+}
int main() {
@@ -479,7 +485,7 @@
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; // ???
//TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt
wait(.1);
@@ -491,9 +497,9 @@
txMsg.id = CAN_MASTER;
txMsg.len = 6;
rxMsg.len = 8;
- can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler
+ can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler ???
- prefs.load(); // Read flash
+ 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
@@ -519,7 +525,7 @@
// =============================================================================================================
// add watch dog here
-// Watchdog &watchdog = Watchdog::get_instance();
+ // Watchdog &watchdog = Watchdog::get_instance();
// =============================================================================================================
state_change = 1;
@@ -568,12 +574,10 @@
wait_us(300);
}
//wait_us(5000);
- }
+ }
else{
// can.write(txMsg);
wait(1.0);
- ;
}
-
}
}