File content as of revision 1:64b881306f6f:

#include "mbed.h"
#include "structs.h"
#include "fw_config.h"
#include "Inverter.h"
#include "foc.h"



//changes to make:
//enable pin, LED
//reshuffle currents and phases
//clockpin dang should have put on a test pad


/*if (BRD_MODEL == 1) {
    DigitalOut clockpin(PA_11); //correct for ESCMK2
    DigitalOut led1(PB_0);      //
    DigitalIn gpio1(PB_1);      // switch
    DigitalOut en_gate(PB_7);  
}*/

//if (BRD_MODEL == 3) {
    DigitalOut clockpin(PB_1); //correct for ESCMK2
    DigitalOut led1(PB_7);      //
    //DigitalIn gpio1(PA_1);      // switch
    DigitalIn gpio1(PB_0);      // switch
    DigitalOut en_gate(PA_12); 
//}

SPI spi(PB_5, PB_4, PB_3); // mosi, miso, sclk
DigitalOut drv_cs(PA_15);  // all correct for ESCMK2
DigitalIn drv_fault(PB_6);  //correct
AnalogOut dout(PA_5);   //   DAC

DigitalIn forward_button(PA_6);      // forward
DigitalIn backward_button(PA_7);      // backward

// controller modes
#define REST_MODE 0
#define TEST_MODE 4


FocStruct focc;
//InverterStruct inverter;

float theta_offset = 5.424f;


float theta = 0.0f;
float offset_est = 0.0f;
float offset_est_agg = 0.0f;
float fake_theta = 0.0f;

float elec_pos = 0;
float e_rad_sec = 0;
float delta_epos_rev = 0;
float delta_epos_avg = 0;
float elec_pos_smooth = 0;
float smooth_pos_weight = 0.85;
float adv = 0.45;

volatile int count = 0;
volatile int main_int_count = 0;
volatile unsigned int main_int_clock = 0;

float current_setpoint = 3.0f;
float full_amp = 20.0f;

int controller_state = 0;

int a = 0;
int b1 = 0;
int b2 = 0;
int b3 = 0;
int b4 = 0;
float a1 = 0;
float last_pos = 0;
float a2 = 0;


Serial pc(PA_2, PA_3);
Inverter inverter;


void Init_DRV8323();
void get_DRV8323_status();


//data logging crap
int data_count = 0;
#define DATA_LEN 500
short data_array1[DATA_LEN];
short data_array2[DATA_LEN];

void print_data_array_short();
void log_data_short( short input1, short input2 );


// Main 20khz loop Interrupt ///

extern "C" void TIM1_UP_TIM16_IRQHandler(void) {
  
  if (TIM1->SR & TIM_SR_UIF ) {

        
        //ADC1->CR  |= ADC_CR_ADSTART;  //Begin sample and conversion
        main_int_count++;
        main_int_clock++;

        for (volatile int t = 0; t < 10; t++) {}
        
        
        
        inverter.adc2_raw = ADC2->DR;
        inverter.adc1_raw = ADC1->DR;
        clockpin = 1;
        
        
        /// Check state machine state, and run the appropriate function ///
        switch(controller_state){
            case REST_MODE:   //nothing 
                TIM1->CCR1 = 3000*(0.5f);  
                TIM1->CCR2 = 3000*(0.5f);
                TIM1->CCR3 = 3000*(0.5f);  
                count++;
                if(count > 20000){
                    count = 0;                                                    
                    led1 = !led1;
                }
                //focc.theta_elec = 0;             
                //commutate(&focc, focc.theta_elec); 
                
            break;
                
            
            case TEST_MODE: 
                //inverter.GetCurrents(&focc);
                
                float i_b_f = I_SCALE*(float)(inverter.adc2_raw - inverter.adc2_offset);
                int   i_b_i = (inverter.adc2_raw - inverter.adc2_offset);
                TIM1->CCR2 = 3200;
                TIM1->CCR1 = 3200;
                
                
                
                /*
                structure of each test:
                1. make current rise to setpoint, hyst. oscillation range to +1a -a1
                2. begin data logger
                3. log til end of log
                4. when log is done, enter rest mode
                */
                
                //TIM1->CCR1 = 3000*(0.0f);  //unused phase- the one with no shunt on it
                
                //TIM1->CCR2 = 3000*(0.5f);  //phase upon which current will be sensed
                //TIM1->CCR3 = 3000*(0.5f);  //phse which will be brought high
                

                if ( ( !backward_button) && (current_setpoint <= full_amp)) {
                    
                    log_data_short( TIM1->CCR3, i_b_i);
                    
                    if ((data_count >= DATA_LEN) | (data_count <= 40))  {  //no logging happening atm, or beginning of log
                        
                        if (i_b_f < current_setpoint) {
                            TIM1->CCR3 = 0; //pull phase high
                        }
                        else {
                            TIM1->CCR3 = 3200; //pull phase low
                        }
                    }
                    else {                                                //its logging time
                        if (i_b_f > (current_setpoint + 1.5f)) {a = 0;}   //current is too high
                        if (i_b_f < (current_setpoint - 1.5f)) {a = 1;}
                    
                        if (a == 1) {
                            TIM1->CCR3 = 0;    //pull phase high
                        }
                        else {  //a = 0;
                            TIM1->CCR3 = 3200; //pull phase low
                        }
                        
                        
                    }
                    
                
                }
                else {
                    TIM1->CCR3 = 3200;
                    a = 1;
                }
                
                
                
                
                inverter.GetCurrents(&focc);
                
            break;
            


        }  
            
      }
            //b = TIM1->CNT;
  TIM1->SR = 0x0;  
  clockpin = 0;                                                             // reset the status register
}

int main() {
    wait_ms(200);
    pc.baud(256000);
    printf("\rStarting Hardware\n");
    gpio1.mode(PullUp);
    forward_button.mode(PullUp);
    backward_button.mode(PullUp);

    Init_DRV8323();
    
    RCC->APB2ENR |= RCC_APB2ENR_TIM17EN;
    TIM17->PSC = 71;
    wait_ms(100);
    TIM17->CR1 |= TIM_CR1_CEN;
    
    inverter.Init();                     // Setup PWM, ADC
    wait(0.1);
    controller_state = REST_MODE;
    wait(0.1);
    inverter.zero_current();
    wait(0.1);
    pc.printf("ADCs zeroed at ");
    pc.printf("%i, %i \n",inverter.adc1_offset,inverter.adc2_offset);                                                 
    wait(0.1);
    inverter.ADCsync();
    focc.i_d_ref = 0.0f;
    //en_gate = 0;
    
    
    //disable_inverter();
    //DigitalIn ranan(PA_0);
    //controller_state = POS_MODE;
    //controller_state = TORQUE_MODE;
    controller_state = TEST_MODE;
    //controller_state = OFFSET_LEARN_MODE;
    
    while(1) {

        wait_us(2);
        
        while (1==1) {
            
        wait_us(30);
        
        //en_gate = 0;
        //wait(3);
        //en_gate = 1;
        //drv_cs = 1; wait_us(1); drv_cs = 0;
        //spi.write((0x00<<15) + (0x02<<11) + (0x01<<5));  //set control register for 3x PWM
         //drv_cs = 1;
         //Init_DRV8323();
        //wait(3);
        
        //bayley_printf();
        //while (1==1) {}
        while (1==1) {
            //if ((data_count == DATA_LEN) && (e_rad_sec > 40)) {
            if ((data_count == DATA_LEN) ) {
                pc.printf(" 0,0 \n");
                print_data_array_short();
                current_setpoint += 0.33333333333f;
                data_count = 0;
                
                if (current_setpoint > full_amp){
                    pc.printf(" 4000,4000 \n");
                    current_setpoint = 3.0f;
                    led1 = 1;
                    wait(5);
                    led1 = 0;
                    
                }
                
                //wait_ms(50);
            }
            //if (drv_fault) {
            //    pc.printf(" 0,0,0,0,0,0,0 \n");
            //}
        
        }
       
        //printf("%i,  ", TIM3->CNT);
        //printf("%i,  ", TIM1->CCR1);
        //printf("%i, ", TIM3->CNT);
        //printf("%X, ", GPIOA->MODER);
        //if (!gpio1) {
        

        //printf("%f,  ", offset_est); 
        //printf("%f,  ", focc.theta_elec);
        //printf("%i,  ", TIM1->CCR1);
        //printf("%i,  ", count);
        
        printf("%f, ",focc.i_b);
        //printf("%f, ",focc.i_b*100);
        //printf("%f, ",focc.theta_elec*1000);

        //printf("%f, ",focc.v_q);
        //printf("%f, ",focc.v_d);

        //printf("%f, ",focc.v_q);
        //printf("%f, ",focc.v_d);//*/
        printf("%f, ",focc.i_d);
        //printf("%f, ",focc.dtc_u);
        //printf("%f,  ", encoder.GetElecVel()/7/6.28 );
        //printf("%f,  ", a1 );
        printf("%i,  ", ADC1->DR);  
        printf("%i,  ", ADC2->DR);
        //printf("    %i, %i %i", a1, a2, a3);

        printf("\r");
        /*
        printf("%x,  ", ADC12_COMMON->CCR);
        printf("%x,  ", ADC1->SQR1);
        printf("%x,  ", ADC2->SQR1);
        printf("%x,  ", GPIOA->MODER);
        printf("%i,  ", ADC1->SMPR1);
        printf("%i,  ", ADC2->SMPR1);
        printf("\r");
        printf("\r");
        printf("%f, ",focc.i_b);
        printf("%f, ",focc.dtc_u);
        while (1==1) {}*/

        //}
        //if (!drv_fault) {printf(" DRV fault "); clockpin = 1; get_DRV8323_status();while (1==1) {}}



        }

    }
}








void Init_DRV8323() {

    drv_cs = 1;

    spi.format(16, 1);
    spi.frequency(1000000);
    en_gate = 1;
    wait_ms(20);
    en_gate = 0;  //clear residual faults
    wait_us(20);
    en_gate = 1;
    
    wait_ms(20);
    drv_cs = 1; wait_us(1); drv_cs = 0;
    int fault1 = 0;
    fault1 = spi.write((0x01<<15) + (0x00<<11));  //status register 1
    drv_cs = 1; wait_us(1); drv_cs = 0;

    while ((fault1>>10) > 0) {
        fault1 = spi.write((0x01<<15) + (0x00<<11));  //status register 1
        pc.printf("Fault Detected!! 0x00=0x%X ", fault1);wait(2); 
        drv_cs = 1; wait_us(1); drv_cs = 0;
        }
    drv_cs = 1; wait_us(1); drv_cs = 0;
    spi.write((0x00<<15) + (0x02<<11) + (0x01<<5));  //set control register for 3x PWM
    //drv_cs = 1; wait_us(1); drv_cs = 0;
    //fault1 = spi.write((0x01<<15) + (0x02<<11));
    //pc.printf("DRV Setup 0x00=0x%X ",fault1);
    drv_cs = 1; 
    //spi.write((0x00<<15) + (0x05<<11) + (0x03));  //set OCP register for 0.26v OCP
    //drv_cs = 1; wait_us(1); drv_cs = 0;    //enable once bridge is working. 
    
    
    }

void get_DRV8323_status() { 
    wait_ms(20);
    drv_cs = 1; wait_us(1); drv_cs = 0;
    int fault1 = 0;
    fault1 = spi.write((0x01<<15) + (0x00<<11));  //status register 1
    drv_cs = 1; wait_us(50);   
    
    pc.printf("Fault1 0x00=0x%X \n\r",fault1);
    
    fault1 = 0;
    wait_us(1); drv_cs = 0;
    fault1 = spi.write((0x01<<15) + (0x01<<11));  //status register 1
    drv_cs = 1; wait_us(50);   
    
    pc.printf("Fault2 0x00=0x%X \n\r",fault1);
    
}

 
    
    
//hyper speed logger
void log_data_short( short input1, short input2 )  {

        if (data_count < DATA_LEN) {
            data_array1[data_count] = input1;
            data_array2[data_count] = input2;
            data_count++;
        }
}


void print_data_array_short() {

    for( int a = 0; a < DATA_LEN; a++ ) {
        pc.printf("%i, %i",data_array1[a],data_array2[a]);
        pc.printf("\n");
    }
    //pc.printf("\n");
}


void enable_inverter() {
    en_gate = 1;
    }
    
void disable_inverter() {
    en_gate = 1;
    }