![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
LIGHT2
main.cpp
- Committer:
- Lightvalve
- Date:
- 2019-08-27
- Revision:
- 12:6f2531038ea4
- Parent:
- 11:82d8768d7351
- Child:
- 13:747daba9cf59
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; } }