![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
HCB with MPC
Dependencies: mbed Eigen FastPWM
main.cpp
- Committer:
- jobuuu
- Date:
- 2019-08-20
- Revision:
- 5:a4319f79457b
- Parent:
- 2:a1c0a37df760
- Child:
- 6:df07d3491e3a
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 }