File content as of revision 5:a4319f79457b:

#include "mbed.h"
#include "FastPWM.h"
#include "INIT_HW.h"
#include "function_CAN.h"
#include "SPI_EEP_ENC.h"
#include "I2C_AS5510.h"
#include "setting.h"

// dac & check
DigitalOut check(PC_2);
DigitalOut check_2(PC_3);
AnalogOut dac_1(PA_4);
AnalogOut dac_2(PA_5);
//AnalogIn adc3(PC_1);

// pwm
double dtc_v=0.0;
double dtc_w=0.0;

// I2C
I2C i2c(PC_9,PA_8); // SDA, SCL (for K22F)
const int i2c_slave_addr1 =  0x56;
unsigned int value; // 10bit output of reading sensor AS5510

// SPI
SPI eeprom(PB_15, PB_14, PB_13); // EEPROM //(SPI_MOSI, SPI_MISO, SPI_SCK);
DigitalOut eeprom_cs(PB_12);
SPI enc(PC_12,PC_11,PC_10);
DigitalOut enc_cs(PD_2);
DigitalOut indi_led(PA_15);

// UART
Serial pc(PA_9,PA_10); //  _ UART

//CAN
CAN can(PB_8, PB_9, 1000000);
CANMessage msg;

// Variables
double cur = 0.0;
double cur_ref = 0.0;
double cur_ref_old = 0.0;
double cur_ref_diff = 0.0;
double cur_err = 0.0;
double cur_err_int = 0.0;
double cur_err_old = 0.0;
double cur_err_diff = 0.0;

double pos = 0.0;
double pos_ref = 0.0;

double vel;
double vel_ref;

double pres_A;
double pres_B;

double V_out=0.0;
double V_rem=0.0; // for anti-windup
double V_MAX = 12000.0; // Maximum Voltage : 12V = 12000mV

double PWM_out=0.0;

int main()
{
    /*******************************************************
    ***     Initialization
    ********************************************************/
        
    indi_led = 0;
    pc.baud(9600);
    
    //Timer t;
    //t.start();
    //t.stop();
    //pc.printf("The time taken was %f seconds\n",t.read());

    // i2c init
//    i2c.frequency(400 * 1000);          // 0.4 mHz
//    wait_ms(2);                         // Power Up wait
//    look_for_hardware_i2c();            // Hardware present
//    init_as5510(i2c_slave_addr1);

//    // spi init
    eeprom.format(8,3);
    eeprom.frequency(5000000); //5M
    enc.format(8,0);
    enc.frequency(5000000); //5M
    
    // ADC init
    Init_ADC();

    // Pwm init
    Init_PWM();
    TIM4->CR1 ^= TIM_CR1_UDIS;
    
//    //SPI
//    spi_eeprom_ready();
//    spi_eeprom_write(0x1,0x112);
//    spi_eeprom_ready();
//    int i = spi_eeprom_read(0x1);

    // CAN
    can.attach(&CAN_RX_HANDLER);
    
    // spi _ enc
    spi_enc_set_init();
    
    
    /*******************************************************
    ***     Program is operating!
    ********************************************************/
    while(1) {
//        dac_1=0.5;
//        dac_2=0.1;
        check_2 = 1;
        //spi _ eeprom
//        spi_eeprom_ready();
//        spi_eeprom_write(0x0001,0xFFFFFFFF);
//        spi_eeprom_ready();
//        int a=spi_eeprom_read(0x0001);
        //spi _ enc
        int a = spi_enc_read();
//        read_field(i2c_slave_addr1);

        check_2=0;

//        pc.printf("%f\n",PWM_out);
//        pc.printf("%d\n",a1);
        
//        wait(0.01f);
    }
}

/*******************************************************************************
                            TIMER INTERRUPT
*******************************************************************************/

unsigned long CNT_TMR4 = 0;
double FREQ_TMR4 = (double)FREQ_20k;
double DT_TMR4 = (double)DT_20k;
extern "C" void TIM4_IRQHandler(void)
{
    if ( TIM4->SR & TIM_SR_UIF ) {
        /*******************************************************
        ***     Sensor Read & Data Handling 
        ********************************************************/
        
        if((CNT_TMR4%2)==0){
            //spi
    //      eeprom.write(0xff);
    //      eeprom.write(0xff);
    //      ready();
    //      read(1);
    
            //i2c
    ////    read_field(i2c_slave_addr1);
    
            //ADC
            ADC3->CR2  |= 0x40000000;                        // adc _ 12bit
    //      a1=ADC1->DR;
    //      a1=ADC2->DR;
//            int raw_cur = ADC3->DR;
            while((ADC3->SR & 0b10));
                    
            double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz
            double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA
            cur=cur*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur);
        }

        //DAC
//      dac_1 = ADC1->DR;
//      dac_2 = ADC2->DR;
        
        /*******************************************************
        ***     Valve Current Control 
        ********************************************************/
        
        bool FLAG_current_control = false;
        if(FLAG_current_control) {
            cur_err = cur_ref - cur;
            cur_err_int = cur_err_int + cur_err*DT_TMR4;
            cur_err_diff = (cur_err - cur_err_old)*FREQ_TMR4;
            cur_err_old = cur_err;
            
            double R_model = 150.0; // ohm
            double L_model = 0.3;
            double w0 = 2.0*3.14*90.0;
            double KP_I = L_model*w0;
            double KI_I = R_model*w0;
            double KD_I = 0.0;
            
            double FF_gain = 0.0;
            V_out = (int) (KP_I * cur_err + KI_I * cur_err_int + KD_I * cur_err_diff);
//          V_out = V_out + FF_gain * (R_model*I_REF); // Unit : mV
            V_out = V_out + FF_gain * (R_model*cur_ref + L_model*cur_ref_diff); // Unit : mV
            
            double Ka = 5.0/KP_I; 
            if(V_out > V_MAX) {
                V_rem = V_out-V_MAX;
                V_rem = Ka*V_rem;
                V_out = V_MAX;
                cur_err_int = cur_err_int - V_rem*DT_5k;
            } else if(V_out < -V_MAX) {
                V_rem = V_out-(-V_MAX);
                V_rem = Ka*V_rem; 
                V_out = -V_MAX;
                cur_err_int = cur_err_int - V_rem*DT_5k;
            }        
        
        } else {
            // PWM_RAW : -5000.0mV~5000.0mV(full duty)
            double t = (double)CNT_TMR4*DT_TMR4;
            double T = 5.0; 
            
            V_out = 1000.0*sin(2.0*PI*t/T); // Unit : mV
//          if(V_out > 0.0) V_out = 1000.0;
//          else if(V_out < 0.0) V_out = -1000.0; 
        }

        PWM_out= V_out/12000.0; // Full duty : 12000.0mV
        
        // Saturation of output voltage to 5.0V
        if(PWM_out > 0.41667) PWM_out=0.41667; //5.0/12.0 = 0.41667
        else if (PWM_out < -0.41667) PWM_out=-0.41667;
        
        if (PWM_out>0.0) {
            dtc_v=0.0;
            dtc_w=PWM_out;
        } else {
            dtc_v=-PWM_out;
            dtc_w=0.0;
        }
        
        //pwm
        TIM4->CCR2 = (PWM_ARR)*(1.0-dtc_v);
        TIM4->CCR1 = (PWM_ARR)*(1.0-dtc_w);
        
        /*******************************************************
        ***     Data Send (CAN) & Print out (UART)
        ********************************************************/
        if((CNT_TMR4%40)==0){
            msg.id = 50;
            msg.len = 4;
            int temp_CUR = (int)(cur*1000.0);
            msg.data[0]=0x00FF&temp_CUR;
            msg.data[1]=0x00FF&(temp_CUR>>8);
            int temp_PWM = (int)(V_out);
            msg.data[2]=0x00FF&temp_PWM;
            msg.data[3]=0x00FF&(temp_PWM>>8);
            can.write(msg);
        }

//        if((CNT_TMR4%4000)==0){
//            pc.printf("%d\n",a1);
//        }

        /*******************************************************
        ***     Timer Counting & etc.
        ********************************************************/
        CNT_TMR4++; 
    }
    TIM4->SR = 0x0;  // reset the status register
}