File content as of revision 10:83a6baa77a2e:

#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 ///////////////////////////////////////////
State pos;
State vel;
State Vout;
State torq;
State pres_A;
State pres_B;
State cur;

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) {
        
        //spi _ enc
        int a = spi_enc_read();
        //i2c
        read_field(i2c_slave_addr1);

        check_2=0;

        pc.printf("%f\n",1234567);
//        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.sen=cur.sen*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur);

        }

        //DAC
//      dac_1 = ADC1->DR;
//      dac_2 = ADC2->DR;
        
        /*******************************************************
        ***     Valve Control 
        ********************************************************/
        ValveControl(CONTROL_MODE);
        
        double t = (double)CNT_TMR4*DT_TMR4;
        double T = 5.0; 
        V_out = 1000.0*sin(2.0*PI*t/T); // V_out : -5000.0mV~5000.0mV(full duty)
//      if(V_out > 0.0) V_out = 1000.0;
//      else if(V_out < 0.0) V_out = -1000.0; 

        /*******************************************************
        ***     PWM
        ********************************************************/
        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.sen*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
}


/*******************************************************************************
 *  CONTROL MODE
 ******************************************************************************/
enum _CONTROL_MODE{
    //control mode
    MODE_NO_ACT = 0,                                    //0
    MODE_VALVE_OPEN_LOOP,                               //1
    MODE_VALVE_POSITION_CONTROL,                        //2
    
    MODE_JOINT_POSITION_TORQUE_CONTROL_PWM,             //3
    MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION,  //4
    MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING,        //5
    
    MODE_JOINT_POSITION_PRES_CONTROL_PWM,               //6
    MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION,    //7
    MODE_VALVE_POSITION_PRES_CONTROL_LEARNING,          //8
        
    MODE_TEST_CURRENT_CONTROL,                          //9
    MODE_TEST_PWM_CONTROL,                              //10
    
    //utility
    MODE_TORQUE_SENSOR_NULLING = 20,                    //20
    MODE_VALVE_NULLING_AND_DEADZONE_SETTING,            //21
    MODE_FIND_HOME,                                     //22
    MODE_VALVE_GAIN_SETTING,                        //23
    MODE_PRESSURE_SENSOR_NULLING,                       //24
    MODE_PRESSURE_SENSOR_CALIB,                         //25
    MODE_ROTARY_FRICTION_TUNING,                        //26
};

void ValveControl(unsigned int ControlMode)
{
    switch (ControlMode) {
        case MODE_NO_ACT: // 0
            V_out = 0.0;
            break;
        case MODE_VALVE_OPEN_LOOP: // 1
            V_out = Vout.ref;
            break;      
        case MODE_VALVE_POSITION_CONTROL: // 2
            CurrentControl();
            break;  
        case MODE_JOINT_POSITION_TORQUE_CONTROL_PWM: // 3
            V_out = 0.0;
            break;      
        case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION: // 4
            double I_REF_POS_FB = 0.0; // I_REF by Position Feedback
            double I_REF_POS_FF = 0.0; // I_REF by Position Feedforward
            
            // feedback input for position control
            pos.err = pos.ref - pos.sen;
            double alpha_update_vel = 1.0/(1.0+(double)FREQ_TMR4/(2.0*3.1415*50.0)); // f_cutoff : 50Hz
            double err_diff = (pos.err - pos.err_old)*(double)FREQ_5k;
            pos.err_diff = (1.0-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff;
            pos.err_old = pos.err;
            I_REF_POS_FB = 0.001*((double)P_GAIN_JOINT_POSITION * pos.err + (double)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1);
            
            // feedforward input for position control
            double Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s] 
            double K_ff = 1.3;
            double K_v = 0.0;
            if(Vel_Act_Ref > 0) K_v = 1.0/100.0; // open, tuning. (deg/s >> mA)
            if(Vel_Act_Ref < 0) K_v = 1.0/100.0; // close, tuning. (deg/s >> mA) 
            I_REF_POS_FF = K_ff*K_v*Vel_Act_Ref;
                        
            cur.ref = I_REF_POS_FF + I_REF_POS_FB;
            break;  
        case MODE_TEST_CURRENT_CONTROL: // 9
            V_out = 0.0;
            break;  
        case MODE_TEST_PWM_CONTROL: // 10
            V_out = 0.0;
            break;   
        case MODE_FIND_HOME: // 22
            V_out = 0.0;
            break;                                             
        default:
            V_out = 0.0;
            break;
    
    }
}

void CurrentControl() {
    cur.err = cur.ref - cur.sen;
    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;
    }   
}