main.cpp

00001 #include "mbed.h"
00002 #include "structs.h"
00003 #include "fw_config.h"
00004 #include "Inverter.h"
00005 #include "foc.h"
00006 
00007 
00008 
00009 //changes to make:
00010 //enable pin, LED
00011 //reshuffle currents and phases
00012 //clockpin dang should have put on a test pad
00013 
00014 
00015 /*if (BRD_MODEL == 1) {
00016     DigitalOut clockpin(PA_11); //correct for ESCMK2
00017     DigitalOut led1(PB_0);      //
00018     DigitalIn gpio1(PB_1);      // switch
00019     DigitalOut en_gate(PB_7);  
00020 }*/
00021 
00022 //if (BRD_MODEL == 3) {
00023     DigitalOut clockpin(PB_1); //correct for ESCMK2
00024     DigitalOut led1(PB_7);      //
00025     //DigitalIn gpio1(PA_1);      // switch
00026     DigitalIn gpio1(PB_0);      // switch
00027     DigitalOut en_gate(PA_12); 
00028 //}
00029 
00030 SPI spi(PB_5, PB_4, PB_3); // mosi, miso, sclk
00031 DigitalOut drv_cs(PA_15);  // all correct for ESCMK2
00032 DigitalIn drv_fault(PB_6);  //correct
00033 AnalogOut dout(PA_5);   //   DAC
00034 
00035 DigitalIn forward_button(PA_6);      // forward
00036 DigitalIn backward_button(PA_7);      // backward
00037 
00038 // controller modes
00039 #define REST_MODE 0
00040 #define TEST_MODE 4
00041 
00042 
00043 FocStruct focc;
00044 //InverterStruct inverter;
00045 
00046 float theta_offset = 5.424f;
00047 
00048 
00049 float theta = 0.0f;
00050 float offset_est = 0.0f;
00051 float offset_est_agg = 0.0f;
00052 float fake_theta = 0.0f;
00053 
00054 float elec_pos = 0;
00055 float e_rad_sec = 0;
00056 float delta_epos_rev = 0;
00057 float delta_epos_avg = 0;
00058 float elec_pos_smooth = 0;
00059 float smooth_pos_weight = 0.85;
00060 float adv = 0.45;
00061 
00062 volatile int count = 0;
00063 volatile int main_int_count = 0;
00064 volatile unsigned int main_int_clock = 0;
00065 
00066 float current_setpoint = 3.0f;
00067 float full_amp = 20.0f;
00068 
00069 int controller_state = 0;
00070 
00071 int a = 0;
00072 int b1 = 0;
00073 int b2 = 0;
00074 int b3 = 0;
00075 int b4 = 0;
00076 float a1 = 0;
00077 float last_pos = 0;
00078 float a2 = 0;
00079 
00080 
00081 Serial pc(PA_2, PA_3);
00082 Inverter inverter;
00083 
00084 
00085 void Init_DRV8323();
00086 void get_DRV8323_status();
00087 
00088 
00089 //data logging crap
00090 int data_count = 0;
00091 #define DATA_LEN 500
00092 short data_array1[DATA_LEN];
00093 short data_array2[DATA_LEN];
00094 
00095 void print_data_array_short();
00096 void log_data_short( short input1, short input2 );
00097 
00098 
00099 // Main 20khz loop Interrupt ///
00100 
00101 extern "C" void TIM1_UP_TIM16_IRQHandler(void) {
00102   
00103   if (TIM1->SR & TIM_SR_UIF ) {
00104 
00105         
00106         //ADC1->CR  |= ADC_CR_ADSTART;  //Begin sample and conversion
00107         main_int_count++;
00108         main_int_clock++;
00109 
00110         for (volatile int t = 0; t < 10; t++) {}
00111         
00112         
00113         
00114         inverter.adc2_raw = ADC2->DR;
00115         inverter.adc1_raw = ADC1->DR;
00116         clockpin = 1;
00117         
00118         
00119         /// Check state machine state, and run the appropriate function ///
00120         switch(controller_state){
00121             case REST_MODE:   //nothing 
00122                 TIM1->CCR1 = 3000*(0.5f);  
00123                 TIM1->CCR2 = 3000*(0.5f);
00124                 TIM1->CCR3 = 3000*(0.5f);  
00125                 count++;
00126                 if(count > 20000){
00127                     count = 0;                                                    
00128                     led1 = !led1;
00129                 }
00130                 //focc.theta_elec = 0;             
00131                 //commutate(&focc, focc.theta_elec); 
00132                 
00133             break;
00134                 
00135             
00136             case TEST_MODE: 
00137                 //inverter.GetCurrents(&focc);
00138                 
00139                 float i_b_f = I_SCALE*(float)(inverter.adc2_raw - inverter.adc2_offset);
00140                 int   i_b_i = (inverter.adc2_raw - inverter.adc2_offset);
00141                 TIM1->CCR2 = 3200;
00142                 TIM1->CCR1 = 3200;
00143                 
00144                 
00145                 
00146                 /*
00147                 structure of each test:
00148                 1. make current rise to setpoint, hyst. oscillation range to +1a -a1
00149                 2. begin data logger
00150                 3. log til end of log
00151                 4. when log is done, enter rest mode
00152                 */
00153                 
00154                 //TIM1->CCR1 = 3000*(0.0f);  //unused phase- the one with no shunt on it
00155                 
00156                 //TIM1->CCR2 = 3000*(0.5f);  //phase upon which current will be sensed
00157                 //TIM1->CCR3 = 3000*(0.5f);  //phse which will be brought high
00158                 
00159 
00160                 if ( ( !backward_button) && (current_setpoint <= full_amp)) {
00161                     
00162                     log_data_short( TIM1->CCR3, i_b_i);
00163                     
00164                     if ((data_count >= DATA_LEN) | (data_count <= 40))  {  //no logging happening atm, or beginning of log
00165                         
00166                         if (i_b_f < current_setpoint) {
00167                             TIM1->CCR3 = 0; //pull phase high
00168                         }
00169                         else {
00170                             TIM1->CCR3 = 3200; //pull phase low
00171                         }
00172                     }
00173                     else {                                                //its logging time
00174                         if (i_b_f > (current_setpoint + 1.5f)) {a = 0;}   //current is too high
00175                         if (i_b_f < (current_setpoint - 1.5f)) {a = 1;}
00176                     
00177                         if (a == 1) {
00178                             TIM1->CCR3 = 0;    //pull phase high
00179                         }
00180                         else {  //a = 0;
00181                             TIM1->CCR3 = 3200; //pull phase low
00182                         }
00183                         
00184                         
00185                     }
00186                     
00187                 
00188                 }
00189                 else {
00190                     TIM1->CCR3 = 3200;
00191                     a = 1;
00192                 }
00193                 
00194                 
00195                 
00196                 
00197                 inverter.GetCurrents(&focc);
00198                 
00199             break;
00200             
00201 
00202 
00203         }  
00204             
00205       }
00206             //b = TIM1->CNT;
00207   TIM1->SR = 0x0;  
00208   clockpin = 0;                                                             // reset the status register
00209 }
00210 
00211 int main() {
00212     wait_ms(200);
00213     pc.baud(256000);
00214     printf("\rStarting Hardware\n");
00215     gpio1.mode(PullUp);
00216     forward_button.mode(PullUp);
00217     backward_button.mode(PullUp);
00218 
00219     Init_DRV8323();
00220     
00221     RCC->APB2ENR |= RCC_APB2ENR_TIM17EN;
00222     TIM17->PSC = 71;
00223     wait_ms(100);
00224     TIM17->CR1 |= TIM_CR1_CEN;
00225     
00226     inverter.Init();                     // Setup PWM, ADC
00227     wait(0.1);
00228     controller_state = REST_MODE;
00229     wait(0.1);
00230     inverter.zero_current();
00231     wait(0.1);
00232     pc.printf("ADCs zeroed at ");
00233     pc.printf("%i, %i \n",inverter.adc1_offset,inverter.adc2_offset);                                                 
00234     wait(0.1);
00235     inverter.ADCsync();
00236     focc.i_d_ref = 0.0f;
00237     //en_gate = 0;
00238     
00239     
00240     //disable_inverter();
00241     //DigitalIn ranan(PA_0);
00242     //controller_state = POS_MODE;
00243     //controller_state = TORQUE_MODE;
00244     controller_state = TEST_MODE;
00245     //controller_state = OFFSET_LEARN_MODE;
00246     
00247     while(1) {
00248 
00249         wait_us(2);
00250         
00251         while (1==1) {
00252             
00253         wait_us(30);
00254         
00255         //en_gate = 0;
00256         //wait(3);
00257         //en_gate = 1;
00258         //drv_cs = 1; wait_us(1); drv_cs = 0;
00259         //spi.write((0x00<<15) + (0x02<<11) + (0x01<<5));  //set control register for 3x PWM
00260          //drv_cs = 1;
00261          //Init_DRV8323();
00262         //wait(3);
00263         
00264         //bayley_printf();
00265         //while (1==1) {}
00266         while (1==1) {
00267             //if ((data_count == DATA_LEN) && (e_rad_sec > 40)) {
00268             if ((data_count == DATA_LEN) ) {
00269                 pc.printf(" 0,0 \n");
00270                 print_data_array_short();
00271                 current_setpoint += 0.33333333333f;
00272                 data_count = 0;
00273                 
00274                 if (current_setpoint > full_amp){
00275                     pc.printf(" 4000,4000 \n");
00276                     current_setpoint = 3.0f;
00277                     led1 = 1;
00278                     wait(5);
00279                     led1 = 0;
00280                     
00281                 }
00282                 
00283                 //wait_ms(50);
00284             }
00285             //if (drv_fault) {
00286             //    pc.printf(" 0,0,0,0,0,0,0 \n");
00287             //}
00288         
00289         }
00290        
00291         //printf("%i,  ", TIM3->CNT);
00292         //printf("%i,  ", TIM1->CCR1);
00293         //printf("%i, ", TIM3->CNT);
00294         //printf("%X, ", GPIOA->MODER);
00295         //if (!gpio1) {
00296         
00297 
00298         //printf("%f,  ", offset_est); 
00299         //printf("%f,  ", focc.theta_elec);
00300         //printf("%i,  ", TIM1->CCR1);
00301         //printf("%i,  ", count);
00302         
00303         printf("%f, ",focc.i_b);
00304         //printf("%f, ",focc.i_b*100);
00305         //printf("%f, ",focc.theta_elec*1000);
00306 
00307         //printf("%f, ",focc.v_q);
00308         //printf("%f, ",focc.v_d);
00309 
00310         //printf("%f, ",focc.v_q);
00311         //printf("%f, ",focc.v_d);//*/
00312         printf("%f, ",focc.i_d);
00313         //printf("%f, ",focc.dtc_u);
00314         //printf("%f,  ", encoder.GetElecVel()/7/6.28 );
00315         //printf("%f,  ", a1 );
00316         printf("%i,  ", ADC1->DR);  
00317         printf("%i,  ", ADC2->DR);
00318         //printf("    %i, %i %i", a1, a2, a3);
00319 
00320         printf("\r");
00321         /*
00322         printf("%x,  ", ADC12_COMMON->CCR);
00323         printf("%x,  ", ADC1->SQR1);
00324         printf("%x,  ", ADC2->SQR1);
00325         printf("%x,  ", GPIOA->MODER);
00326         printf("%i,  ", ADC1->SMPR1);
00327         printf("%i,  ", ADC2->SMPR1);
00328         printf("\r");
00329         printf("\r");
00330         printf("%f, ",focc.i_b);
00331         printf("%f, ",focc.dtc_u);
00332         while (1==1) {}*/
00333 
00334         //}
00335         //if (!drv_fault) {printf(" DRV fault "); clockpin = 1; get_DRV8323_status();while (1==1) {}}
00336 
00337 
00338 
00339         }
00340 
00341     }
00342 }
00343 
00344 
00345 
00346 
00347 
00348 
00349 
00350 
00351 void Init_DRV8323() {
00352 
00353     drv_cs = 1;
00354 
00355     spi.format(16, 1);
00356     spi.frequency(1000000);
00357     en_gate = 1;
00358     wait_ms(20);
00359     en_gate = 0;  //clear residual faults
00360     wait_us(20);
00361     en_gate = 1;
00362     
00363     wait_ms(20);
00364     drv_cs = 1; wait_us(1); drv_cs = 0;
00365     int fault1 = 0;
00366     fault1 = spi.write((0x01<<15) + (0x00<<11));  //status register 1
00367     drv_cs = 1; wait_us(1); drv_cs = 0;
00368 
00369     while ((fault1>>10) > 0) {
00370         fault1 = spi.write((0x01<<15) + (0x00<<11));  //status register 1
00371         pc.printf("Fault Detected!! 0x00=0x%X ", fault1);wait(2); 
00372         drv_cs = 1; wait_us(1); drv_cs = 0;
00373         }
00374     drv_cs = 1; wait_us(1); drv_cs = 0;
00375     spi.write((0x00<<15) + (0x02<<11) + (0x01<<5));  //set control register for 3x PWM
00376     //drv_cs = 1; wait_us(1); drv_cs = 0;
00377     //fault1 = spi.write((0x01<<15) + (0x02<<11));
00378     //pc.printf("DRV Setup 0x00=0x%X ",fault1);
00379     drv_cs = 1; 
00380     //spi.write((0x00<<15) + (0x05<<11) + (0x03));  //set OCP register for 0.26v OCP
00381     //drv_cs = 1; wait_us(1); drv_cs = 0;    //enable once bridge is working. 
00382     
00383     
00384     }
00385 
00386 void get_DRV8323_status() { 
00387     wait_ms(20);
00388     drv_cs = 1; wait_us(1); drv_cs = 0;
00389     int fault1 = 0;
00390     fault1 = spi.write((0x01<<15) + (0x00<<11));  //status register 1
00391     drv_cs = 1; wait_us(50);   
00392     
00393     pc.printf("Fault1 0x00=0x%X \n\r",fault1);
00394     
00395     fault1 = 0;
00396     wait_us(1); drv_cs = 0;
00397     fault1 = spi.write((0x01<<15) + (0x01<<11));  //status register 1
00398     drv_cs = 1; wait_us(50);   
00399     
00400     pc.printf("Fault2 0x00=0x%X \n\r",fault1);
00401     
00402 }
00403 
00404  
00405     
00406     
00407 //hyper speed logger
00408 void log_data_short( short input1, short input2 )  {
00409 
00410         if (data_count < DATA_LEN) {
00411             data_array1[data_count] = input1;
00412             data_array2[data_count] = input2;
00413             data_count++;
00414         }
00415 }
00416 
00417 
00418 void print_data_array_short() {
00419 
00420     for( int a = 0; a < DATA_LEN; a++ ) {
00421         pc.printf("%i, %i",data_array1[a],data_array2[a]);
00422         pc.printf("\n");
00423     }
00424     //pc.printf("\n");
00425 }
00426 
00427 
00428 void enable_inverter() {
00429     en_gate = 1;
00430     }
00431     
00432 void disable_inverter() {
00433     en_gate = 1;
00434     }
00435