nhan pham an
Fork and fix for mwork
Dependencies: mbed-dev-f303 FastPWM3 millis
Diff: main.cpp
- Revision:
- 47:e1196a851f76
- Parent:
- 46:2d4b1dafcfe3
- Child:
- 48:74a40481740c
--- a/main.cpp Thu Jul 12 02:50:34 2018 +0000
+++ b/main.cpp Wed Dec 05 04:07:46 2018 +0000
@@ -2,6 +2,7 @@
/// Written by benkatz, with much inspiration from Bayley Wang, Nick Kirkby, Shane Colton, 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
+/// Version for the TI DRV8323 Everything Chip
#define REST_MODE 0
#define CALIBRATION_MODE 1
@@ -37,7 +38,6 @@
GPIOStruct gpio;
ControllerStruct controller;
COMStruct com;
-ObserverStruct observer;
Serial pc(PA_2, PA_3);
@@ -59,7 +59,7 @@
void onMsgReceived() {
//msgAvailable = true;
- //printf("%.3f %.3f %.3f\n\r", controller.theta_mech, controller.dtheta_mech, controller.i_q);
+ printf("%df\n\r", rxMsg.id);
can.read(rxMsg);
if((rxMsg.id == CAN_ID)){
controller.timeout = 0;
@@ -86,6 +86,7 @@
void enter_menu_state(void){
drv.disable_gd();
+ //gpio.enable->write(0);
printf("\n\r\n\r\n\r");
printf(" Commands:\n\r");
wait_us(10);
@@ -102,7 +103,6 @@
printf(" esc - Exit to Menu\n\r");
wait_us(10);
state_change = 0;
- //gpio.enable->write(0);
gpio.led->write(0);
}
@@ -128,6 +128,7 @@
void enter_torque_mode(void){
drv.enable_gd();
+ //gpio.enable->write(1);
controller.ovp_flag = 0;
reset_foc(&controller); // Tesets integrators, and other control loop parameters
wait(.001);
@@ -140,6 +141,7 @@
void calibrate(void){
drv.enable_gd();
+ //gpio.enable->write(1);
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
@@ -147,12 +149,14 @@
wait(.2);
printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r");
drv.disable_gd();
+ //gpio.enable->write(0);
state_change = 0;
}
void print_encoder(void){
- printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
- wait(.05);
+ //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 ///
@@ -165,7 +169,7 @@
//volatile int delay;
//for (delay = 0; delay < 55; delay++);
- spi.Sample(); // sample position sensor
+ spi.Sample(DT); // sample position sensor
controller.adc2_raw = ADC2->DR; // Read ADC Data Registers
controller.adc1_raw = ADC1->DR;
controller.adc3_raw = ADC3->DR;
@@ -173,7 +177,7 @@
controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();
controller.dtheta_elec = spi.GetElecVelocity();
- controller.v_bus = 24.0f;//0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE;
+ controller.v_bus = V_BUS;//b0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE;
///
/// Check state machine state, and run the appropriate function ///
@@ -198,7 +202,8 @@
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);
+ gpio.
+ ->write(0);
controller.ovp_flag = 1;
state = REST_MODE;
state_change = 1;
@@ -206,7 +211,7 @@
}
*/
- torque_control(&controller);
+ //torque_control(&controller);
if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
controller.i_d_ref = 0;
controller.i_q_ref = 0;
@@ -214,24 +219,46 @@
controller.kd = 0;
controller.t_ff = 0;
}
+ //controller.i_q_ref = 0.0f;
+ //controller.i_d_ref = 0.0f;
+
//controller.i_q_ref = 2.0f;
- //controller.i_d_ref = 30.0f;
- commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop
+ //controller.i_d_ref = -30.0f;
+ //controller.kd = .1;
+ //controller.v_des = 25;
+ torque_control(&controller);
+ //controller.i_q_ref = 2.0f;
+ //controller.i_d_ref = -20.0f;
+ commutate(&controller, &gpio, controller.theta_elec); // Run current loop
//TIM1->CCR3 = (PWM_ARR)*(0.5f); // Write duty cycles
//TIM1->CCR2 = (PWM_ARR)*(0.5f);
//TIM1->CCR1 = (PWM_ARR)*(0.5f);
controller.timeout += 1;
+
+ //pc.putc(iq);
+ //pc.putc(id);
+ //pc.putc(ang);
+ //pc.printf("\n\r");
count++;
+
/*
- if(count == 4000){
+ if(count == 400){
//printf("%d %d %.4f %.4f %.4f\n\r", controller.adc1_raw, controller.adc2_raw, controller.i_a, controller.i_b, controller.i_c);
- printf("%.3f\n\r", controller.dtheta_mech);
+ printf("%.2f\n\r", 1000.0f*controller.h1_iq_afc_int1);
+ //pc.putc(id);
+ //pc.putc(iq);
+ //pc.putc(ang);
+ //pc.putc('\n');
+ //pc.putc('\r');
+ //printf("%.1f\n", controller.k_q*controller.ki_q*controller.q_int);
count = 0;
}
*/
+
+
@@ -288,7 +315,7 @@
break;
case 'z':
spi.SetMechOffset(0);
- spi.Sample();
+ spi.Sample(DT);
wait_us(20);
M_OFFSET = spi.GetMechPosition();
if (!prefs.ready()) prefs.open();
@@ -370,11 +397,11 @@
wait_us(100);
drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x0, 0x0, 0x0, SEN_LVL_1_0);
wait_us(100);
- drv.write_OCPCR(TRETRY_4MS, DEADTIME_100NS, OCP_RETRY, OCP_DEG_4US, VDS_LVL_0_75);
+ drv.write_OCPCR(TRETRY_4MS, DEADTIME_200NS, OCP_RETRY, OCP_DEG_4US, VDS_LVL_1_5);
//drv.enable_gd();
zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset
- //drv.disable_gd();
+ drv.disable_gd();
@@ -426,12 +453,12 @@
- printf(" %d\n\r", drv.read_register(DCR));
- wait_us(100);
- printf(" %d\n\r", drv.read_register(CSACR));
- wait_us(100);
- printf(" %d\n\r", drv.read_register(OCPCR));
- drv.disable_gd();
+ //printf(" %d\n\r", drv.read_register(DCR));
+ //wait_us(100);
+ //printf(" %d\n\r", drv.read_register(CSACR));
+ //wait_us(100);
+ //printf(" %d\n\r", drv.read_register(OCPCR));
+ //drv.disable_gd();
pc.attach(&serial_interrupt); // attach serial interrupt
@@ -440,5 +467,13 @@
int counter = 0;
while(1) {
+ drv.print_faults();
+ wait(.1);
+ if(state == MOTOR_MODE)
+ {
+ //printf("%.3f\n\r", controller.dtheta_mech);
+ //wait(.001);
+ }
+
}
}