Sungwoo Kim / Mbed 2 deprecated HydraulicControlBoard_LIGHT

For LIGHT

Dependencies:   mbed FastPWM

main.cpp@7:e9086c72bb22, 2019-08-20 (annotated)

Committer:
jobuuu
Date:
Tue Aug 20 12:27:19 2019 +0000
Revision:
7:e9086c72bb22
Parent:
6:df07d3491e3a
Child:
8:5d2eebdad025
Updated : 20190820;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GiJeongKim 0:51c43836c1d7 1 #include "mbed.h"
GiJeongKim 0:51c43836c1d7 2 #include "FastPWM.h"
GiJeongKim 0:51c43836c1d7 3 #include "INIT_HW.h"
jobuuu 2:a1c0a37df760 4 #include "function_CAN.h"
GiJeongKim 0:51c43836c1d7 5 #include "SPI_EEP_ENC.h"
GiJeongKim 0:51c43836c1d7 6 #include "I2C_AS5510.h"
GiJeongKim 0:51c43836c1d7 7 #include "setting.h"
GiJeongKim 0:51c43836c1d7 8
jobuuu 7:e9086c72bb22 9 // dac & check ///////////////////////////////////////////
GiJeongKim 0:51c43836c1d7 10 DigitalOut check(PC_2);
GiJeongKim 0:51c43836c1d7 11 DigitalOut check_2(PC_3);
GiJeongKim 0:51c43836c1d7 12 AnalogOut dac_1(PA_4);
GiJeongKim 0:51c43836c1d7 13 AnalogOut dac_2(PA_5);
jobuuu 5:a4319f79457b 14 //AnalogIn adc3(PC_1);
GiJeongKim 0:51c43836c1d7 15
jobuuu 7:e9086c72bb22 16 // PWM ///////////////////////////////////////////
jobuuu 2:a1c0a37df760 17 double dtc_v=0.0;
jobuuu 2:a1c0a37df760 18 double dtc_w=0.0;
GiJeongKim 0:51c43836c1d7 19
jobuuu 7:e9086c72bb22 20 // I2C ///////////////////////////////////////////
GiJeongKim 0:51c43836c1d7 21 I2C i2c(PC_9,PA_8); // SDA, SCL (for K22F)
GiJeongKim 0:51c43836c1d7 22 const int i2c_slave_addr1 = 0x56;
GiJeongKim 0:51c43836c1d7 23 unsigned int value; // 10bit output of reading sensor AS5510
GiJeongKim 0:51c43836c1d7 24
jobuuu 7:e9086c72bb22 25 // SPI ///////////////////////////////////////////
GiJeongKim 0:51c43836c1d7 26 SPI eeprom(PB_15, PB_14, PB_13); // EEPROM //(SPI_MOSI, SPI_MISO, SPI_SCK);
GiJeongKim 0:51c43836c1d7 27 DigitalOut eeprom_cs(PB_12);
GiJeongKim 0:51c43836c1d7 28 SPI enc(PC_12,PC_11,PC_10);
GiJeongKim 0:51c43836c1d7 29 DigitalOut enc_cs(PD_2);
jobuuu 2:a1c0a37df760 30 DigitalOut indi_led(PA_15);
GiJeongKim 0:51c43836c1d7 31
jobuuu 7:e9086c72bb22 32 // UART ///////////////////////////////////////////
GiJeongKim 0:51c43836c1d7 33 Serial pc(PA_9,PA_10); // _ UART
GiJeongKim 0:51c43836c1d7 34
jobuuu 7:e9086c72bb22 35 // CAN ///////////////////////////////////////////
GiJeongKim 0:51c43836c1d7 36 CAN can(PB_8, PB_9, 1000000);
GiJeongKim 0:51c43836c1d7 37 CANMessage msg;
jobuuu 2:a1c0a37df760 38
jobuuu 7:e9086c72bb22 39 // Variables ///////////////////////////////////////////
jobuuu 7:e9086c72bb22 40 State pos;
jobuuu 7:e9086c72bb22 41 State vel;
jobuuu 7:e9086c72bb22 42 State Vout;
jobuuu 7:e9086c72bb22 43 State torq;
jobuuu 7:e9086c72bb22 44 State pres_A;
jobuuu 7:e9086c72bb22 45 State pres_B;
jobuuu 7:e9086c72bb22 46 State cur;
jobuuu 5:a4319f79457b 47
jobuuu 5:a4319f79457b 48 double V_out=0.0;
jobuuu 5:a4319f79457b 49 double V_rem=0.0; // for anti-windup
jobuuu 5:a4319f79457b 50 double V_MAX = 12000.0; // Maximum Voltage : 12V = 12000mV
jobuuu 5:a4319f79457b 51
jobuuu 5:a4319f79457b 52 double PWM_out=0.0;
jobuuu 5:a4319f79457b 53
jobuuu 7:e9086c72bb22 54 // =============================================================================
jobuuu 7:e9086c72bb22 55 // =============================================================================
jobuuu 7:e9086c72bb22 56 // =============================================================================
jobuuu 7:e9086c72bb22 57
GiJeongKim 0:51c43836c1d7 58 int main()
GiJeongKim 0:51c43836c1d7 59 {
jobuuu 6:df07d3491e3a 60 /*********************************
jobuuu 1:e04e563be5ce 61 *** Initialization
jobuuu 6:df07d3491e3a 62 *********************************/
jobuuu 1:e04e563be5ce 63
jobuuu 2:a1c0a37df760 64 indi_led = 0;
jobuuu 1:e04e563be5ce 65 pc.baud(9600);
jobuuu 2:a1c0a37df760 66
jobuuu 2:a1c0a37df760 67 //Timer t;
jobuuu 2:a1c0a37df760 68 //t.start();
jobuuu 2:a1c0a37df760 69 //t.stop();
jobuuu 2:a1c0a37df760 70 //pc.printf("The time taken was %f seconds\n",t.read());
jobuuu 2:a1c0a37df760 71
GiJeongKim 0:51c43836c1d7 72 // i2c init
GiJeongKim 0:51c43836c1d7 73 // i2c.frequency(400 * 1000); // 0.4 mHz
GiJeongKim 0:51c43836c1d7 74 // wait_ms(2); // Power Up wait
GiJeongKim 0:51c43836c1d7 75 // look_for_hardware_i2c(); // Hardware present
GiJeongKim 0:51c43836c1d7 76 // init_as5510(i2c_slave_addr1);
jobuuu 2:a1c0a37df760 77
GiJeongKim 0:51c43836c1d7 78 // // spi init
GiJeongKim 0:51c43836c1d7 79 eeprom.format(8,3);
GiJeongKim 0:51c43836c1d7 80 eeprom.frequency(5000000); //5M
GiJeongKim 0:51c43836c1d7 81 enc.format(8,0);
GiJeongKim 0:51c43836c1d7 82 enc.frequency(5000000); //5M
jobuuu 2:a1c0a37df760 83
GiJeongKim 0:51c43836c1d7 84 // ADC init
jobuuu 5:a4319f79457b 85 Init_ADC();
jobuuu 2:a1c0a37df760 86
GiJeongKim 0:51c43836c1d7 87 // Pwm init
GiJeongKim 0:51c43836c1d7 88 Init_PWM();
GiJeongKim 0:51c43836c1d7 89 TIM4->CR1 ^= TIM_CR1_UDIS;
jobuuu 2:a1c0a37df760 90
GiJeongKim 0:51c43836c1d7 91 // //SPI
GiJeongKim 0:51c43836c1d7 92 // spi_eeprom_ready();
GiJeongKim 0:51c43836c1d7 93 // spi_eeprom_write(0x1,0x112);
GiJeongKim 0:51c43836c1d7 94 // spi_eeprom_ready();
GiJeongKim 0:51c43836c1d7 95 // int i = spi_eeprom_read(0x1);
jobuuu 2:a1c0a37df760 96
GiJeongKim 0:51c43836c1d7 97 // CAN
jobuuu 2:a1c0a37df760 98 can.attach(&CAN_RX_HANDLER);
jobuuu 2:a1c0a37df760 99
GiJeongKim 0:51c43836c1d7 100 // spi _ enc
GiJeongKim 0:51c43836c1d7 101 spi_enc_set_init();
jobuuu 1:e04e563be5ce 102
jobuuu 1:e04e563be5ce 103
jobuuu 6:df07d3491e3a 104 /************************************
jobuuu 1:e04e563be5ce 105 *** Program is operating!
jobuuu 6:df07d3491e3a 106 *************************************/
GiJeongKim 0:51c43836c1d7 107 while(1) {
GiJeongKim 0:51c43836c1d7 108 // dac_1=0.5;
GiJeongKim 0:51c43836c1d7 109 // dac_2=0.1;
GiJeongKim 0:51c43836c1d7 110 check_2 = 1;
GiJeongKim 0:51c43836c1d7 111 //spi _ eeprom
GiJeongKim 0:51c43836c1d7 112 // spi_eeprom_ready();
GiJeongKim 0:51c43836c1d7 113 // spi_eeprom_write(0x0001,0xFFFFFFFF);
GiJeongKim 0:51c43836c1d7 114 // spi_eeprom_ready();
GiJeongKim 0:51c43836c1d7 115 // int a=spi_eeprom_read(0x0001);
GiJeongKim 0:51c43836c1d7 116 //spi _ enc
GiJeongKim 0:51c43836c1d7 117 int a = spi_enc_read();
GiJeongKim 0:51c43836c1d7 118 // read_field(i2c_slave_addr1);
jobuuu 2:a1c0a37df760 119
GiJeongKim 0:51c43836c1d7 120 check_2=0;
jobuuu 2:a1c0a37df760 121
jobuuu 2:a1c0a37df760 122 // pc.printf("%f\n",PWM_out);
jobuuu 2:a1c0a37df760 123 // pc.printf("%d\n",a1);
jobuuu 2:a1c0a37df760 124
jobuuu 2:a1c0a37df760 125 // wait(0.01f);
GiJeongKim 0:51c43836c1d7 126 }
jobuuu 1:e04e563be5ce 127 }
jobuuu 1:e04e563be5ce 128
jobuuu 6:df07d3491e3a 129
jobuuu 6:df07d3491e3a 130
jobuuu 6:df07d3491e3a 131
jobuuu 2:a1c0a37df760 132 /*******************************************************************************
jobuuu 2:a1c0a37df760 133 TIMER INTERRUPT
jobuuu 2:a1c0a37df760 134 *******************************************************************************/
jobuuu 2:a1c0a37df760 135
jobuuu 2:a1c0a37df760 136 unsigned long CNT_TMR4 = 0;
jobuuu 2:a1c0a37df760 137 double FREQ_TMR4 = (double)FREQ_20k;
jobuuu 5:a4319f79457b 138 double DT_TMR4 = (double)DT_20k;
jobuuu 1:e04e563be5ce 139 extern "C" void TIM4_IRQHandler(void)
jobuuu 1:e04e563be5ce 140 {
jobuuu 2:a1c0a37df760 141 if ( TIM4->SR & TIM_SR_UIF ) {
jobuuu 1:e04e563be5ce 142 /*******************************************************
jobuuu 1:e04e563be5ce 143 *** Sensor Read & Data Handling
jobuuu 1:e04e563be5ce 144 ********************************************************/
jobuuu 1:e04e563be5ce 145
jobuuu 2:a1c0a37df760 146 if((CNT_TMR4%2)==0){
jobuuu 2:a1c0a37df760 147 //spi
jobuuu 2:a1c0a37df760 148 // eeprom.write(0xff);
jobuuu 2:a1c0a37df760 149 // eeprom.write(0xff);
jobuuu 2:a1c0a37df760 150 // ready();
jobuuu 2:a1c0a37df760 151 // read(1);
jobuuu 2:a1c0a37df760 152
jobuuu 2:a1c0a37df760 153 //i2c
jobuuu 2:a1c0a37df760 154 //// read_field(i2c_slave_addr1);
jobuuu 2:a1c0a37df760 155
jobuuu 2:a1c0a37df760 156 //ADC
jobuuu 5:a4319f79457b 157 ADC3->CR2 |= 0x40000000; // adc _ 12bit
jobuuu 2:a1c0a37df760 158 // a1=ADC1->DR;
jobuuu 2:a1c0a37df760 159 // a1=ADC2->DR;
jobuuu 5:a4319f79457b 160 // int raw_cur = ADC3->DR;
jobuuu 5:a4319f79457b 161 while((ADC3->SR & 0b10));
jobuuu 5:a4319f79457b 162
jobuuu 5:a4319f79457b 163 double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz
jobuuu 5:a4319f79457b 164 double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA
jobuuu 7:e9086c72bb22 165 cur.sen=cur.sen*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur);
jobuuu 7:e9086c72bb22 166
jobuuu 2:a1c0a37df760 167 }
jobuuu 1:e04e563be5ce 168
jobuuu 1:e04e563be5ce 169 //DAC
jobuuu 1:e04e563be5ce 170 // dac_1 = ADC1->DR;
jobuuu 1:e04e563be5ce 171 // dac_2 = ADC2->DR;
jobuuu 1:e04e563be5ce 172
jobuuu 1:e04e563be5ce 173 /*******************************************************
jobuuu 6:df07d3491e3a 174 *** Valve Control
jobuuu 1:e04e563be5ce 175 ********************************************************/
jobuuu 7:e9086c72bb22 176 ValveControl(CONTROL_MODE);
jobuuu 1:e04e563be5ce 177
jobuuu 7:e9086c72bb22 178 double t = (double)CNT_TMR4*DT_TMR4;
jobuuu 7:e9086c72bb22 179 double T = 5.0;
jobuuu 7:e9086c72bb22 180 V_out = 1000.0*sin(2.0*PI*t/T); // V_out : -5000.0mV~5000.0mV(full duty)
jobuuu 7:e9086c72bb22 181 // if(V_out > 0.0) V_out = 1000.0;
jobuuu 7:e9086c72bb22 182 // else if(V_out < 0.0) V_out = -1000.0;
jobuuu 5:a4319f79457b 183
jobuuu 7:e9086c72bb22 184 /*******************************************************
jobuuu 7:e9086c72bb22 185 *** PWM
jobuuu 7:e9086c72bb22 186 ********************************************************/
jobuuu 5:a4319f79457b 187 PWM_out= V_out/12000.0; // Full duty : 12000.0mV
jobuuu 2:a1c0a37df760 188
jobuuu 2:a1c0a37df760 189 // Saturation of output voltage to 5.0V
jobuuu 5:a4319f79457b 190 if(PWM_out > 0.41667) PWM_out=0.41667; //5.0/12.0 = 0.41667
jobuuu 5:a4319f79457b 191 else if (PWM_out < -0.41667) PWM_out=-0.41667;
jobuuu 2:a1c0a37df760 192
jobuuu 2:a1c0a37df760 193 if (PWM_out>0.0) {
jobuuu 2:a1c0a37df760 194 dtc_v=0.0;
jobuuu 1:e04e563be5ce 195 dtc_w=PWM_out;
jobuuu 1:e04e563be5ce 196 } else {
jobuuu 2:a1c0a37df760 197 dtc_v=-PWM_out;
jobuuu 2:a1c0a37df760 198 dtc_w=0.0;
jobuuu 1:e04e563be5ce 199 }
jobuuu 1:e04e563be5ce 200
jobuuu 1:e04e563be5ce 201 //pwm
jobuuu 2:a1c0a37df760 202 TIM4->CCR2 = (PWM_ARR)*(1.0-dtc_v);
jobuuu 2:a1c0a37df760 203 TIM4->CCR1 = (PWM_ARR)*(1.0-dtc_w);
jobuuu 1:e04e563be5ce 204
jobuuu 1:e04e563be5ce 205 /*******************************************************
jobuuu 2:a1c0a37df760 206 *** Data Send (CAN) & Print out (UART)
jobuuu 1:e04e563be5ce 207 ********************************************************/
jobuuu 2:a1c0a37df760 208 if((CNT_TMR4%40)==0){
jobuuu 2:a1c0a37df760 209 msg.id = 50;
jobuuu 2:a1c0a37df760 210 msg.len = 4;
jobuuu 7:e9086c72bb22 211 int temp_CUR = (int)(cur.sen*1000.0);
jobuuu 2:a1c0a37df760 212 msg.data[0]=0x00FF&temp_CUR;
jobuuu 2:a1c0a37df760 213 msg.data[1]=0x00FF&(temp_CUR>>8);
jobuuu 5:a4319f79457b 214 int temp_PWM = (int)(V_out);
jobuuu 2:a1c0a37df760 215 msg.data[2]=0x00FF&temp_PWM;
jobuuu 2:a1c0a37df760 216 msg.data[3]=0x00FF&(temp_PWM>>8);
jobuuu 2:a1c0a37df760 217 can.write(msg);
jobuuu 2:a1c0a37df760 218 }
jobuuu 1:e04e563be5ce 219
jobuuu 2:a1c0a37df760 220 // if((CNT_TMR4%4000)==0){
jobuuu 2:a1c0a37df760 221 // pc.printf("%d\n",a1);
jobuuu 2:a1c0a37df760 222 // }
jobuuu 1:e04e563be5ce 223
jobuuu 1:e04e563be5ce 224 /*******************************************************
jobuuu 1:e04e563be5ce 225 *** Timer Counting & etc.
jobuuu 1:e04e563be5ce 226 ********************************************************/
jobuuu 2:a1c0a37df760 227 CNT_TMR4++;
jobuuu 1:e04e563be5ce 228 }
jobuuu 1:e04e563be5ce 229 TIM4->SR = 0x0; // reset the status register
jobuuu 7:e9086c72bb22 230 }
jobuuu 7:e9086c72bb22 231
jobuuu 7:e9086c72bb22 232
jobuuu 7:e9086c72bb22 233 /*******************************************************************************
jobuuu 7:e9086c72bb22 234 * CONTROL MODE
jobuuu 7:e9086c72bb22 235 ******************************************************************************/
jobuuu 7:e9086c72bb22 236 enum _CONTROL_MODE{
jobuuu 7:e9086c72bb22 237 //control mode
jobuuu 7:e9086c72bb22 238 MODE_NO_ACT = 0, //0
jobuuu 7:e9086c72bb22 239 MODE_VALVE_OPEN_LOOP, //1
jobuuu 7:e9086c72bb22 240 MODE_VALVE_POSITION_CONTROL, //2
jobuuu 7:e9086c72bb22 241
jobuuu 7:e9086c72bb22 242 MODE_JOINT_POSITION_TORQUE_CONTROL_PWM, //3
jobuuu 7:e9086c72bb22 243 MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION, //4
jobuuu 7:e9086c72bb22 244 MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING, //5
jobuuu 7:e9086c72bb22 245
jobuuu 7:e9086c72bb22 246 MODE_JOINT_POSITION_PRES_CONTROL_PWM, //6
jobuuu 7:e9086c72bb22 247 MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION, //7
jobuuu 7:e9086c72bb22 248 MODE_VALVE_POSITION_PRES_CONTROL_LEARNING, //8
jobuuu 7:e9086c72bb22 249
jobuuu 7:e9086c72bb22 250 MODE_TEST_CURRENT_CONTROL, //9
jobuuu 7:e9086c72bb22 251 MODE_TEST_PWM_CONTROL, //10
jobuuu 7:e9086c72bb22 252
jobuuu 7:e9086c72bb22 253 //utility
jobuuu 7:e9086c72bb22 254 MODE_TORQUE_SENSOR_NULLING = 20, //20
jobuuu 7:e9086c72bb22 255 MODE_VALVE_NULLING_AND_DEADZONE_SETTING, //21
jobuuu 7:e9086c72bb22 256 MODE_FIND_HOME, //22
jobuuu 7:e9086c72bb22 257 MODE_VALVE_GAIN_SETTING, //23
jobuuu 7:e9086c72bb22 258 MODE_PRESSURE_SENSOR_NULLING, //24
jobuuu 7:e9086c72bb22 259 MODE_PRESSURE_SENSOR_CALIB, //25
jobuuu 7:e9086c72bb22 260 MODE_ROTARY_FRICTION_TUNING, //26
jobuuu 7:e9086c72bb22 261 };
jobuuu 7:e9086c72bb22 262
jobuuu 7:e9086c72bb22 263 void ValveControl(unsigned int ControlMode)
jobuuu 7:e9086c72bb22 264 {
jobuuu 7:e9086c72bb22 265 switch (ControlMode) {
jobuuu 7:e9086c72bb22 266 case MODE_NO_ACT: // 0
jobuuu 7:e9086c72bb22 267 V_out = 0.0;
jobuuu 7:e9086c72bb22 268 break;
jobuuu 7:e9086c72bb22 269 case MODE_VALVE_OPEN_LOOP: // 1
jobuuu 7:e9086c72bb22 270 V_out = Vout.ref;
jobuuu 7:e9086c72bb22 271 break;
jobuuu 7:e9086c72bb22 272 case MODE_VALVE_POSITION_CONTROL: // 2
jobuuu 7:e9086c72bb22 273 CurrentControl();
jobuuu 7:e9086c72bb22 274 break;
jobuuu 7:e9086c72bb22 275 case MODE_JOINT_POSITION_TORQUE_CONTROL_PWM: // 3
jobuuu 7:e9086c72bb22 276 V_out = 0.0;
jobuuu 7:e9086c72bb22 277 break;
jobuuu 7:e9086c72bb22 278 case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION: // 4
jobuuu 7:e9086c72bb22 279 double I_REF_POS_FB = 0.0; // I_REF by Position Feedback
jobuuu 7:e9086c72bb22 280 double I_REF_POS_FF = 0.0; // I_REF by Position Feedforward
jobuuu 7:e9086c72bb22 281
jobuuu 7:e9086c72bb22 282 // feedback input for position control
jobuuu 7:e9086c72bb22 283 pos.err = pos.ref - pos.sen;
jobuuu 7:e9086c72bb22 284 double alpha_update_vel = 1.0/(1.0+(double)FREQ_TMR4/(2.0*3.1415*50.0)); // f_cutoff : 50Hz
jobuuu 7:e9086c72bb22 285 double err_diff = (pos.err - pos.err_old)*(double)FREQ_5k;
jobuuu 7:e9086c72bb22 286 pos.err_diff = (1.0-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff;
jobuuu 7:e9086c72bb22 287 pos.err_old = pos.err;
jobuuu 7:e9086c72bb22 288 I_REF_POS_FB = 0.001*((double)P_GAIN_JOINT_POSITION * pos.err + (double)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1);
jobuuu 7:e9086c72bb22 289
jobuuu 7:e9086c72bb22 290 // feedforward input for position control
jobuuu 7:e9086c72bb22 291 double Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
jobuuu 7:e9086c72bb22 292 double K_ff = 1.3;
jobuuu 7:e9086c72bb22 293 double K_v = 0.0;
jobuuu 7:e9086c72bb22 294 if(Vel_Act_Ref > 0) K_v = 1.0/100.0; // open, tuning. (deg/s >> mA)
jobuuu 7:e9086c72bb22 295 if(Vel_Act_Ref < 0) K_v = 1.0/100.0; // close, tuning. (deg/s >> mA)
jobuuu 7:e9086c72bb22 296 I_REF_POS_FF = K_ff*K_v*Vel_Act_Ref;
jobuuu 7:e9086c72bb22 297
jobuuu 7:e9086c72bb22 298 cur.ref = I_REF_POS_FF + I_REF_POS_FB;
jobuuu 7:e9086c72bb22 299 break;
jobuuu 7:e9086c72bb22 300 case MODE_TEST_CURRENT_CONTROL: // 9
jobuuu 7:e9086c72bb22 301 V_out = 0.0;
jobuuu 7:e9086c72bb22 302 break;
jobuuu 7:e9086c72bb22 303 case MODE_TEST_PWM_CONTROL: // 10
jobuuu 7:e9086c72bb22 304 V_out = 0.0;
jobuuu 7:e9086c72bb22 305 break;
jobuuu 7:e9086c72bb22 306 case MODE_FIND_HOME: // 22
jobuuu 7:e9086c72bb22 307 V_out = 0.0;
jobuuu 7:e9086c72bb22 308 break;
jobuuu 7:e9086c72bb22 309 default:
jobuuu 7:e9086c72bb22 310 V_out = 0.0;
jobuuu 7:e9086c72bb22 311 break;
jobuuu 7:e9086c72bb22 312
jobuuu 7:e9086c72bb22 313 }
jobuuu 7:e9086c72bb22 314 }
jobuuu 7:e9086c72bb22 315
jobuuu 7:e9086c72bb22 316 void CurrentControl() {
jobuuu 7:e9086c72bb22 317 cur.err = cur.ref - cur.sen;
jobuuu 7:e9086c72bb22 318 cur.err_int = cur.err_int + cur.err*DT_TMR4;
jobuuu 7:e9086c72bb22 319 cur.err_diff = (cur.err - cur.err_old)*FREQ_TMR4;
jobuuu 7:e9086c72bb22 320 cur.err_old = cur.err;
jobuuu 7:e9086c72bb22 321
jobuuu 7:e9086c72bb22 322 double R_model = 150.0; // ohm
jobuuu 7:e9086c72bb22 323 double L_model = 0.3;
jobuuu 7:e9086c72bb22 324 double w0 = 2.0*3.14*90.0;
jobuuu 7:e9086c72bb22 325 double KP_I = L_model*w0;
jobuuu 7:e9086c72bb22 326 double KI_I = R_model*w0;
jobuuu 7:e9086c72bb22 327 double KD_I = 0.0;
jobuuu 7:e9086c72bb22 328
jobuuu 7:e9086c72bb22 329 double FF_gain = 0.0;
jobuuu 7:e9086c72bb22 330 V_out = (int) (KP_I * cur.err + KI_I * cur.err_int + KD_I * cur.err_diff);
jobuuu 7:e9086c72bb22 331 // V_out = V_out + FF_gain * (R_model*I_REF); // Unit : mV
jobuuu 7:e9086c72bb22 332 V_out = V_out + FF_gain * (R_model*cur.ref + L_model*cur.ref_diff); // Unit : mV
jobuuu 7:e9086c72bb22 333
jobuuu 7:e9086c72bb22 334 double Ka = 5.0/KP_I;
jobuuu 7:e9086c72bb22 335 if(V_out > V_MAX) {
jobuuu 7:e9086c72bb22 336 V_rem = V_out-V_MAX;
jobuuu 7:e9086c72bb22 337 V_rem = Ka*V_rem;
jobuuu 7:e9086c72bb22 338 V_out = V_MAX;
jobuuu 7:e9086c72bb22 339 cur.err_int = cur.err_int - V_rem*DT_5k;
jobuuu 7:e9086c72bb22 340 } else if(V_out < -V_MAX) {
jobuuu 7:e9086c72bb22 341 V_rem = V_out-(-V_MAX);
jobuuu 7:e9086c72bb22 342 V_rem = Ka*V_rem;
jobuuu 7:e9086c72bb22 343 V_out = -V_MAX;
jobuuu 7:e9086c72bb22 344 cur.err_int = cur.err_int - V_rem*DT_5k;
jobuuu 7:e9086c72bb22 345 }
GiJeongKim 0:51c43836c1d7 346 }