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