File content as of revision 12:6f2531038ea4:

#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"
#include "function_utilities.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 LED(PA_15);

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

// CAN ///////////////////////////////////////////
CAN can(PB_8, PB_9, 1000000);
CANMessage msg;
void onMsgReceived()
{
    CAN_RX_HANDLER();
}

// 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 ID_index_array[100] = {0};

double CUR_PRES_DIFF_BAR = 0;

double CUR_PRES_A_sum = 0.0;
double CUR_PRES_B_sum = 0.0;
double CUR_PRES_A_mean = 0.0;
double CUR_PRES_B_mean = 0.0;
double CUR_TORQUE_sum = 0.0;
double CUR_TORQUE_mean = 0.0;
double PRES_A_NULL = 200.0;
double PRES_B_NULL = 200.0;
double TORQUE_NULL = 3900;
// =============================================================================
// =============================================================================
// =============================================================================

/*******************************************************************************
 *  REFERENCE MODE
 ******************************************************************************/
enum _REFERENCE_MODE{
    MODE_REF_NO_ACT = 0,                                //0 
    MODE_REF_DIRECT,                                //1
    MODE_REF_COS_INC,                                  //2
    MODE_REF_LINE_INC,                                 //3
    MODE_REF_SIN_WAVE,                                  //4
    MODE_REF_SQUARE_WAVE,                                  //5
};

/*******************************************************************************
 *  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
    
    MODE_DDV_POS_VS_PWM_ID = 30,                           //30
    MODE_DDV_DEADZONE_AND_CENTER,                       //31
    MODE_DDV_POS_VS_FLOWRATE,                           //32
};

int main()
{
    /*********************************
    ***     Initialization
    *********************************/
        
    LED = 1;
    pc.baud(9600);

    // 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);
    make_delay();

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

    // Pwm init
    Init_PWM();
    TIM4->CR1 ^= TIM_CR1_UDIS;
    make_delay();
    
    // TMR3 init
    Init_TMR3();
    TIM3->CR1 ^= TIM_CR1_UDIS;
    make_delay();

    // CAN
    can.attach(&CAN_RX_HANDLER);
    CAN_ID_INIT();
    make_delay();
    
    // spi _ enc
    spi_enc_set_init();
    make_delay();
    
    //eeprom
    ROM_INIT_DATA();
    make_delay();
    
    //DAC init
    dac_1 = PRES_A_VREF/3.3;
    dac_2 = PRES_B_VREF/3.3;
    make_delay();
    
    for (int i=0; i<100; i++){
        if(i%2==0)
            ID_index_array[i] = - i * 0.5;
        else
            ID_index_array[i] =  (i+1) * 0.5;
    }
    
    
    /************************************
    ***     Program is operating!
    *************************************/
    while(1) {
        
        //spi _ enc
        //int a = spi_enc_read();
        
        //i2c
        read_field(i2c_slave_addr1);
    }
}




/*******************************************************************************
                            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){
    
            //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;

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

unsigned long CNT_TMR3 = 0;
double FREQ_TMR3 = (double)FREQ_5k;
double DT_TMR3 = (double)DT_5k;
extern "C" void TIM3_IRQHandler(void)
{
    if ( TIM3->SR & TIM_SR_UIF ) {
        
        ENC_UPDATE();
        CUR_PRES_A_BAR = (CUR_PRES_A - PRES_A_NULL) / PRES_SENSOR_A_PULSE_PER_BAR + 1.;
        CUR_PRES_B_BAR = (CUR_PRES_B - PRES_B_NULL) / PRES_SENSOR_B_PULSE_PER_BAR + 1.;
        //CUR_TORQUE_NM = (CUR_TORQUE - TORQUE_NULL) / (double) TORQUE_SENSOR_PULSE_PER_TORQUE; //
        CUR_TORQUE_NM = CUR_TORQUE; //
        CUR_TORQUE_NM_PRESS = 1. * (CUR_PRES_A_BAR - CUR_PRES_B_BAR);
        
            // Reference Loop
        switch (REFERENCE_MODE) {
            case MODE_REF_NO_ACT:
            {
                break;
            }
            
            case MODE_REF_DIRECT:
            {
                if (FLAG_REFERENCE_VALVE_PWM) {
                    Ref_PWM = (double) REF_PWM;
                }
                if (FLAG_REFERENCE_VALVE_POSITION) {
                    Ref_Valve_Pos = (double) REF_VALVE_POSITION;
                }
                if (FLAG_REFERENCE_JOINT_POSITION) {
                    Ref_Joint_Pos = (double) REF_POSITION;
                    Ref_Joint_Vel = (double) REF_VELOCITY;
                }
                if (FLAG_REFERENCE_JOINT_TORQUE) {
                    Ref_Joint_Torq = (double) REF_TORQUE;
                }
                break;
            }
            
            case MODE_REF_COS_INC:
            {
                if (FLAG_REFERENCE_VALVE_PWM) {
                    Ref_PWM = ((double) REF_PWM - (double) INIT_REF_PWM)*(0.5 - 0.5 * cos(3.14159 * (double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_REF_PWM;
                }
                if (FLAG_REFERENCE_VALVE_POSITION) {
                    Ref_Valve_Pos = ((double) REF_VALVE_POSITION - (double) INIT_REF_VALVE_POS)*(0.5 - 0.5 * cos(3.14159 * (double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_REF_VALVE_POS;
                }
                if (FLAG_REFERENCE_JOINT_POSITION) {
                    Ref_Joint_Pos = ((double) REF_POSITION - (double) INIT_REF_POS)*(0.5 - 0.5 * cos(3.14159 * (double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_REF_POS;
                    Ref_Joint_Vel = 0.0;
                }
                if (FLAG_REFERENCE_JOINT_TORQUE) {
                    Ref_Joint_Torq = ((double) REF_TORQUE - (double) INIT_REF_TORQUE)*(0.5 - 0.5 * cos(3.14159 * (double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_REF_TORQUE;
                }
                TMR2_COUNT_REFERENCE++;
                if (TMR2_COUNT_REFERENCE >= REF_MOVE_TIME_5k) {
                    REFERENCE_MODE = MODE_REF_DIRECT;
                    TMR2_COUNT_REFERENCE = 0;
                }
                break;
            }
         
            
            case MODE_REF_LINE_INC:
            {
                if (FLAG_REFERENCE_VALVE_PWM) {
                    Ref_PWM = ((double) REF_PWM - (double) INIT_REF_PWM)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_PWM;
                }
                if (FLAG_REFERENCE_VALVE_POSITION) {
                    Ref_Valve_Pos = ((double) REF_VALVE_POSITION - (double) INIT_REF_VALVE_POS)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_VALVE_POS;
                }
                if (FLAG_REFERENCE_JOINT_POSITION) {
                    Ref_Joint_Pos = ((double) REF_POSITION - (double) INIT_REF_POS)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_POS;
                    Ref_Vel_Test = ((double) REF_POSITION - (double) INIT_REF_POS) / (double) REF_MOVE_TIME_5k * (double) TMR_FREQ_5k;    //   pulse/sec
                    //Ref_Vel_Test = 10;
                }
                if (FLAG_REFERENCE_JOINT_TORQUE) {
                    Ref_Joint_Torq = ((double) REF_TORQUE - (double) INIT_REF_TORQUE)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_TORQUE;
                }
                TMR2_COUNT_REFERENCE++;
                if (TMR2_COUNT_REFERENCE >= REF_MOVE_TIME_5k) {
                    //REFERENCE_MODE = MODE_REF_DIRECT;
                    Ref_Vel_Test = 1;
                    TMR2_COUNT_REFERENCE = REF_MOVE_TIME_5k;
                    //TMR2_COUNT_REFERENCE = 0;
                }
                break;
            }
            
            case MODE_REF_SIN_WAVE:
            {
                if (FLAG_REFERENCE_VALVE_PWM) {
                    Ref_PWM = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_PWM;
                }
                if (FLAG_REFERENCE_VALVE_POSITION) {
                    Ref_Valve_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_VALVE_POSITION;
                }
                if (FLAG_REFERENCE_JOINT_POSITION) {
                    Ref_Joint_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_POSITION;
                }
                if (FLAG_REFERENCE_JOINT_TORQUE) {
                    Ref_Joint_Torq = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_TORQUE;
                }
                TMR2_COUNT_REFERENCE++;
                if (TMR2_COUNT_REFERENCE >= TMR2_COUNT_REFERENCE * REF_NUM) {
                    REFERENCE_MODE = MODE_REF_DIRECT;
                    TMR2_COUNT_REFERENCE = 0;
                }
                break;
            }
            
            case MODE_REF_SQUARE_WAVE:
            {
                if (FLAG_REFERENCE_VALVE_PWM) {
                    Ref_PWM = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_PWM;
                    if (Ref_PWM >= REF_PWM) Ref_PWM = REF_MAG + REF_PWM;
                    else Ref_PWM = -REF_MAG + REF_PWM;
                }
                if (FLAG_REFERENCE_VALVE_POSITION) {
                    Ref_Valve_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_VALVE_POSITION;
                    if (Ref_Valve_Pos >= REF_VALVE_POSITION) Ref_Valve_Pos = REF_MAG + REF_VALVE_POSITION;
                    else Ref_Valve_Pos = -REF_MAG + REF_VALVE_POSITION;
                }
                if (FLAG_REFERENCE_JOINT_POSITION) {
                    Ref_Joint_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_POSITION;
                    if (Ref_Joint_Pos >= REF_POSITION) Ref_Valve_Pos = REF_MAG + REF_POSITION;
                    else Ref_Joint_Pos = -REF_MAG + REF_POSITION;
                }
                if (FLAG_REFERENCE_JOINT_TORQUE) {
                    Ref_Joint_Torq = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_TORQUE;
                    if (Ref_Joint_Torq >= REF_TORQUE) Ref_Valve_Pos = REF_MAG + REF_TORQUE;
                    else Ref_Joint_Torq = -REF_MAG + REF_TORQUE;
                }
                TMR2_COUNT_REFERENCE++;
                if (TMR2_COUNT_REFERENCE >= TMR2_COUNT_REFERENCE * REF_NUM) {
                    REFERENCE_MODE = MODE_REF_DIRECT;
                    TMR2_COUNT_REFERENCE = 0;
                }
                break;
            }
            
            default:
                break;
        }
        
        
        
        /*******************************************************
        ***     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_TMR3%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_TMR3%5000)==0){
            if(LED==1)
            {
                LED=0;
            }
            else
                LED = 1;
//            LED != LED;
        }

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

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;
    }   
}