Gu Jasper
1
Dependencies: mbed-dev-f303 FastPWM3
Revision 56:d34e4540ec12, committed 2020-08-14
- Comitter:
- Jasper_gu
- Date:
- Fri Aug 14 07:22:11 2020 +0000
- Parent:
- 55:a6a9a8a68f2d
- Commit message:
- uuuu
Changed in this revision
--- a/Config/hw_config.h Fri Aug 02 02:39:16 2019 +0000 +++ b/Config/hw_config.h Fri Aug 14 07:22:11 2020 +0000 @@ -1,11 +1,11 @@ #ifndef HW_CONFIG_H #define HW_CONFIG_H -#define PIN_U PA_10 +#define PIN_U PA_8 #define PIN_V PA_9 -#define PIN_W PA_8 -#define ENABLE_PIN PA_11 // Enable gate drive pin -#define LED PC_5 // LED Pin +#define PIN_W PA_10 +#define ENABLE_PIN PC_9 // Enable gate drive pin +#define LED PC_7 // LED Pin #define I_SCALE 0.02014160156f // Amps per A/D Count #define V_SCALE 0.012890625f // Bus volts per A/D Count #define DTC_MAX 0.94f // Max phase duty cycle
--- a/Config/motor_config.h Fri Aug 02 02:39:16 2019 +0000 +++ b/Config/motor_config.h Fri Aug 14 07:22:11 2020 +0000 @@ -7,11 +7,11 @@ #define L_D 0.00002f //Henries #define L_Q 0.00002f //Henries #define KT .08f //N-m per peak phase amp, = WB*NPP*3/2 -//#define NPP 21 //Number of pole pairs -#define NPP 5 //Number of pole pairs -//#define GR 1.0f //Gear ratio -#define GR 49.0f //Gear ratio -#define KT_OUT 0.45f //KT*GR +#define NPP 21 //Number of pole pairs +//#define NPP 5 //Number of pole pairs +#define GR 1.0f //Gear ratio +//#define GR 49.0f //Gear ratio +#define KT_OUT 0.08f //KT*GR #define WB 0.0025f //Flux linkage, Webers. #define R_TH 1.25f //Kelvin per watt #define INV_M_TH 0.03125f //Kelvin per joule
--- a/DRV8323/DRV.cpp Fri Aug 02 02:39:16 2019 +0000
+++ b/DRV8323/DRV.cpp Fri Aug 14 07:22:11 2020 +0000
@@ -6,6 +6,8 @@
_cs = cs;
_cs->write(1);
wait_us(10);
+ GPIOC->AFR[1]&=0xfff000ff;
+ GPIOC->AFR[1]|=0x00066600;
_spi->format(16, 1);
_spi->frequency(500000);
}
--- a/FOC/foc.cpp Fri Aug 02 02:39:16 2019 +0000
+++ b/FOC/foc.cpp Fri Aug 14 07:22:11 2020 +0000
@@ -143,7 +143,7 @@
controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
}
else{
- controller->i_b = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
+ controller->i_b = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
controller->i_c = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);
}
controller->i_a = -controller->i_b - controller->i_c;
@@ -201,7 +201,9 @@
linearize_dtc(&dtc_d);
linearize_dtc(&dtc_q);
controller->v_d = dtc_d*controller->v_bus;
+ controller->v_d=0;
controller->v_q = dtc_q*controller->v_bus;
+ controller->v_q = 2;
abc(controller->theta_elec + 0.0f*DT*controller->dtheta_elec, controller->v_d, controller->v_q, &controller->v_u, &controller->v_v, &controller->v_w); //inverse dq0 transform on voltages
svm(controller->v_bus, controller->v_u, controller->v_v, controller->v_w, &controller->dtc_u, &controller->dtc_v, &controller->dtc_w); //space vector modulation
@@ -213,7 +215,7 @@
else{
TIM1->CCR3 = (PWM_ARR)*(1.0f-controller->dtc_u);
TIM1->CCR1 = (PWM_ARR)*(1.0f-controller->dtc_v);
- TIM1->CCR2 = (PWM_ARR)*(1.0f-controller->dtc_w);
+ TIM1->CCR2 = (PWM_ARR)*(1.0f-controller->dtc_w);
}
controller->theta_elec = theta;
@@ -240,7 +242,8 @@
//===============================HJB ended===================================//
//float torque_ref = controller->kp*(controller->p_des - controller->theta_mech) + controller->kd*(controller->v_des - controller->dtheta_mech)+ controller->t_ff ; //HJB changed
// 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_q_ref=0.5;
controller->i_d_ref = 0.0f;
}
\ No newline at end of file
--- a/PositionSensor/PositionSensor.cpp Fri Aug 02 02:39:16 2019 +0000
+++ b/PositionSensor/PositionSensor.cpp Fri Aug 14 07:22:11 2020 +0000
@@ -7,15 +7,46 @@
PositionSensorAM5147::PositionSensorAM5147(int CPR, float offset, int ppairs){
//_CPR = CPR;
+
_CPR = CPR;
_ppairs = ppairs;
ElecOffset = offset;
rotations = 0;
- spi = new SPI(PC_12, PC_11, PC_10);
- spi->format(16, 1); // mbed v>127 breaks 16-bit spi, so transaction is broken into 2 8-bit words
- spi->frequency(25000000);
+ // spi = new SPI(PB_5, PB_4, PB_3);
+ //使能相关时钟
+ __GPIOB_CLK_ENABLE();
+ __SPI1_CLK_ENABLE();
+//设置为复用输出
+
+ GPIOB->MODER&=0xFFFFF03F;
+ GPIOB->MODER|=0x00000A80;
+ GPIOB->OSPEEDR&=0xFFFFF03F;
+ GPIOB->OSPEEDR|=0x00000A80;
+ GPIOB->OTYPER&=0x0000ffc7;
+
+ GPIOB->AFR[0]&=0xff000fff;
+ GPIOB->AFR[0]|=0x00555000;
+ GPIOA->AFR[0]&=0x000fffff;
- cs = new DigitalOut(PA_15);
+ uint16_t tempreg=0;
+ tempreg|=0<<10;//全双工
+ tempreg|=1<<9;//软件NSS
+ tempreg|=1<<8;//
+ tempreg|=1<<2; //SPI主机
+ tempreg|=1<<11;//16位数据格式
+ tempreg|=0<<1; //空闲时SCK为1
+ tempreg|=1<<0; //第二个边沿采样
+ tempreg|=7<<3;//256分频
+ tempreg|=0<<7;//MSB
+ tempreg|=1<<6;//SPI使能
+ SPI1->CR1=tempreg;
+ SPI1->I2SCFGR&=~(1<<11);
+ //
+ // spi->format(16, 1); // mbed v>127 breaks 16-bit spi, so transaction is broken into 2 8-bit words
+ // spi->frequency(1000000);
+
+
+ cs = new DigitalOut(PD_2);
cs->write(1);
readAngleCmd = 0xffff;
MechOffset = offset;
@@ -27,18 +58,25 @@
pre_raw = 0;
counti=0;
}
-
+uint16_t PositionSensorAM5147::Spi1_ReadWriteByte(uint16_t Txdata)
+{
+ while((SPI1->SR&1<<1)==0);
+ SPI1->DR=Txdata;
+ while((SPI1->SR&1<<0)==0);
+ return SPI1->DR;
+}
void PositionSensorAM5147::Sample(float dt){
pre2_raw = pre_raw; //====HJB added== for compare, filter the extra error
pre_raw = raw; //====HJB added==
- GPIOA->ODR &= ~(1 << 15);
- raw = spi->write(readAngleCmd);
+ GPIOD->ODR &= ~(1 << 2);
+ //raw = spi->write(readAngleCmd);
+ raw=Spi1_ReadWriteByte(readAngleCmd);
raw &= 0x3FFF;
//raw = spi->write(0);
//raw = raw>>1; //Extract last 14 bits
//raw = raw>>2; //Extract last 14 bits
- GPIOA->ODR |= (1 << 15);
+ GPIOD->ODR |= (1 << 2);
/*
if(16300>=abs(raw-pre_raw)>=100)
{
--- a/PositionSensor/PositionSensor.h Fri Aug 02 02:39:16 2019 +0000
+++ b/PositionSensor/PositionSensor.h Fri Aug 14 07:22:11 2020 +0000
@@ -63,6 +63,7 @@
DigitalOut *cs;
int readAngleCmd;
int offset_lut[128];
+ uint16_t Spi1_ReadWriteByte(uint16_t Txdata);
};
--- a/hw_setup.cpp Fri Aug 02 02:39:16 2019 +0000
+++ b/hw_setup.cpp Fri Aug 14 07:22:11 2020 +0000
@@ -50,16 +50,16 @@
ADC->CCR = 0x00000016; // Regular simultaneous mode only
ADC1->CR2 |= ADC_CR2_ADON;//0x00000001; // ADC1 ON
- ADC1->SQR3 = 0x000000A; // use PC_0 as input- ADC1_IN0
+ ADC1->SQR3 = 0x000000D; // use PC_3 as input- ADC1_IN13
ADC2->CR2 |= ADC_CR2_ADON;//0x00000001; // ADC2 ON
- ADC2->SQR3 = 0x0000000B; // use PC_1 as input - ADC2_IN11
+ ADC2->SQR3 = 0x0000000C; // use PC_2 as input - ADC2_IN12
ADC3->CR2 |= ADC_CR2_ADON; // ADC3 ON
ADC3->SQR3 = 0x00000000; // use PA_0, - ADC3_IN0
- GPIOC->MODER |= 0x0000000f; // Alternate function, PC_0, PC_1 are analog inputs
+ GPIOC->MODER |= 0x000000F0; // Alternate function, PC_2, PC_3 are analog inputs
GPIOA->MODER |= 0x3; // PA_0 as analog input
- ADC1->SMPR1 |= 0x1; // 15 cycles on CH_10, 0b 001
- ADC2->SMPR1 |= 0x8; // 15 cycles on CH_11, 0b 0001 000
+ ADC1->SMPR1 |= 0x200; // 15 cycles on CH_13, 0b 001 000 000 000
+ ADC2->SMPR1 |= 0x40; // 15 cycles on CH_12, 0b 001 000 000
ADC3->SMPR2 |= 0x1; // 15 cycles on CH_0, 0b 001;
--- a/main.cpp Fri Aug 02 02:39:16 2019 +0000
+++ b/main.cpp Fri Aug 14 07:22:11 2020 +0000
@@ -46,13 +46,18 @@
CANMessage rxMsg;
CANMessage txMsg;
+PositionSensorAM5147 spi(16384, 0.0, NPP); //resolution is 0.02197265625 deg
-SPI drv_spi(PA_7, PA_6, PA_5);
-DigitalOut drv_cs(PA_4);
-//DigitalOut drv_en_gate(PA_11);
-DRV832x drv(&drv_spi, &drv_cs);
+//drv8323 SPI 设置 改为PC12\11\10 SPI3,片选改为PA15
+SPI drv_spi(PC_12, PC_11, PC_10);
-PositionSensorAM5147 spi(16384, 0.0, NPP); //resolution is 0.02197265625 deg
+// GPIOC->AFR[1]&=0xfff000ff;
+// GPIOC->AFR[1]|=0x00055500;
+DigitalOut drv_cs(PA_15);
+
+DRV832x drv(&drv_spi, &drv_cs);
+
+
volatile int count = 0;
volatile int state = REST_MODE;
@@ -142,7 +147,7 @@
//gpio.enable->write(1);
controller.ovp_flag = 0;
controller.init_flag = 0; //HJB added. The flag of fastest way to go to the desire position, state change or CAN time off.
- reset_foc(&controller); // Tesets integrators, and other control loop parameters
+ reset_foc(&controller); // Resets integrators, and other control loop parameters
wait(.001);
controller.i_d_ref = 0;
controller.i_q_ref = 0; // Current Setpoints
@@ -167,7 +172,9 @@
void print_encoder(void){
printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition()); //spi.GetMechPosition
+ // printf("%d %d %d \n\r",controller.adc1_raw,controller.adc2_raw,controller.adc3_raw);
//printf("%d\n\r", spi.GetRawPosition());
+ // can.write(txMsg);
wait(.001);
}
@@ -207,7 +214,7 @@
break;
case MOTOR_MODE: // Run torque control
- if(state_change){
+ if(state_change){//Initiate Torque control parameters
enter_torque_mode();
count = 0;
//===============================================HJB added====================================================//
@@ -246,7 +253,7 @@
controller.v_des = 0; //HJB added
}
- torque_control(&controller);
+ torque_control(&controller);//Calculate the desire torque for current loop
commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop
controller.timeout++;
@@ -277,7 +284,7 @@
void serial_interrupt(void){
while(pc.readable()){
char c = pc.getc();
- if(c == 27){
+ if(c == 27){ //return to default mode
//===============================================HJB added====================================================//
wait_us(100); //HJB add, to clear fault
drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1); //HJB add, to clear fault
@@ -304,11 +311,11 @@
state = ENCODER_MODE;
state_change = 1;
break;
- case 's':
+ case 's'://
state = SETUP_MODE;
state_change = 1;
break;
- case 'z':
+ case 'z'://set zero position
spi.SetMechOffset(0);
spi.Sample(DT);
wait_us(20);
@@ -396,7 +403,7 @@
gpio.enable->write(1); //enable!
wait_us(100);
- drv.calibrate();
+ drv.calibrate(); //calibrete and initialize the Drv8323 Chip
wait_us(100);
drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1);
wait_us(100);
@@ -419,7 +426,7 @@
TIM1->CCR1 = 0x708*(1.0f);
gpio.enable->write(0);
*/
- reset_foc(&controller); // Reset current controller
+ reset_foc(&controller); // Reset current controller
reset_observer(&observer); // Reset observer
TIM1->CR1 ^= TIM_CR1_UDIS;
//TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt
@@ -477,7 +484,7 @@
state_change = 1;
- int counter = 0;
+ // int counter = 0;
//===============================================HJB added====================================================//
wait_us(100); //HJB add, to clear fault
drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1); //HJB add, to clear fault
@@ -485,19 +492,22 @@
//===============================================HJB ended====================================================//
while(1) {
drv.print_faults();
- wait(.1);
+ wait(.1);
//printf("%.4f\n\r", controller.v_bus);
if(state == MOTOR_MODE)
{
//printf(" %.3f %.3f %.3f \n\r", controller.p_des,controller.p_des-controller.theta_mech,controller.v_des-controller.dtheta_mech); //spi.GetMechPosition
- printf(" %.3f %.3f %.3f \n\r", controller.v_des*60/(2*PI),controller.dtheta_mech*60/(2*PI),controller.v_des-controller.dtheta_mech); //spi.GetMechPosition
+ printf(" %.3f %d %d \n\r", controller.v_bus,controller.adc1_raw,controller.adc2_raw); //spi.GetMechPosition
// 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\n\r", controller.dtheta_mech);
wait(.002);
}
-
+// spi.Sample(DT);
+// print_encoder();
+ wait(0.01);
+ // printf(" %d\n\r", drv.read_register(DCR));
/*
if(state == MOTOR_MODE)
{
--- a/structs.h Fri Aug 02 02:39:16 2019 +0000
+++ b/structs.h Fri Aug 14 07:22:11 2020 +0000
@@ -21,7 +21,7 @@
float v_bus; // DC link voltage
float theta_mech, theta_elec; // Rotor mechanical and electrical angle
float dtheta_mech, dtheta_elec, dtheta_elec_filt; // Rotor mechanical and electrical angular velocit
- float i_d, i_q, i_q_filt, i_d_filt; // D/Q currents
+ float i_d, i_q, i_q_filt, i_d_filt; // D/Q currents
float v_d, v_q; // D/Q voltages
float dtc_u, dtc_v, dtc_w; // Terminal duty cycles
float v_u, v_v, v_w; // Terminal voltages