Sungwoo Kim / Mbed 2 deprecated HydraulicControlBoard_Base

distribution-201229

Dependencies:   mbed FastPWM

main.cpp@9:7f07aa6ff49a, 2019-08-23 (annotated)

Committer:
Lightvalve
Date:
Fri Aug 23 01:00:24 2019 +0000
Revision:
9:7f07aa6ff49a
Parent:
8:5d2eebdad025
Child:
10:83a6baa77a2e
adsfa

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
Lightvalve 8:5d2eebdad025 73 i2c.frequency(400 * 1000); // 0.4 mHz
Lightvalve 8:5d2eebdad025 74 wait_ms(2); // Power Up wait
Lightvalve 8:5d2eebdad025 75 look_for_hardware_i2c(); // Hardware present
Lightvalve 8:5d2eebdad025 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) {
Lightvalve 9:7f07aa6ff49a 108
GiJeongKim 0:51c43836c1d7 109 //spi _ enc
GiJeongKim 0:51c43836c1d7 110 int a = spi_enc_read();
Lightvalve 9:7f07aa6ff49a 111 //i2c
Lightvalve 8:5d2eebdad025 112 read_field(i2c_slave_addr1);
jobuuu 2:a1c0a37df760 113
GiJeongKim 0:51c43836c1d7 114 check_2=0;
jobuuu 2:a1c0a37df760 115
Lightvalve 8:5d2eebdad025 116 pc.printf("%f\n",1234);
jobuuu 2:a1c0a37df760 117 // pc.printf("%d\n",a1);
jobuuu 2:a1c0a37df760 118
jobuuu 2:a1c0a37df760 119 // wait(0.01f);
GiJeongKim 0:51c43836c1d7 120 }
jobuuu 1:e04e563be5ce 121 }
jobuuu 1:e04e563be5ce 122
jobuuu 6:df07d3491e3a 123
jobuuu 6:df07d3491e3a 124
jobuuu 6:df07d3491e3a 125
jobuuu 2:a1c0a37df760 126 /*******************************************************************************
jobuuu 2:a1c0a37df760 127 TIMER INTERRUPT
jobuuu 2:a1c0a37df760 128 *******************************************************************************/
jobuuu 2:a1c0a37df760 129
jobuuu 2:a1c0a37df760 130 unsigned long CNT_TMR4 = 0;
jobuuu 2:a1c0a37df760 131 double FREQ_TMR4 = (double)FREQ_20k;
jobuuu 5:a4319f79457b 132 double DT_TMR4 = (double)DT_20k;
jobuuu 1:e04e563be5ce 133 extern "C" void TIM4_IRQHandler(void)
jobuuu 1:e04e563be5ce 134 {
jobuuu 2:a1c0a37df760 135 if ( TIM4->SR & TIM_SR_UIF ) {
jobuuu 1:e04e563be5ce 136 /*******************************************************
jobuuu 1:e04e563be5ce 137 *** Sensor Read & Data Handling
jobuuu 1:e04e563be5ce 138 ********************************************************/
jobuuu 1:e04e563be5ce 139
jobuuu 2:a1c0a37df760 140 if((CNT_TMR4%2)==0){
jobuuu 2:a1c0a37df760 141 //spi
jobuuu 2:a1c0a37df760 142 // eeprom.write(0xff);
jobuuu 2:a1c0a37df760 143 // eeprom.write(0xff);
jobuuu 2:a1c0a37df760 144 // ready();
jobuuu 2:a1c0a37df760 145 // read(1);
jobuuu 2:a1c0a37df760 146
jobuuu 2:a1c0a37df760 147 //i2c
jobuuu 2:a1c0a37df760 148 //// read_field(i2c_slave_addr1);
jobuuu 2:a1c0a37df760 149
jobuuu 2:a1c0a37df760 150 //ADC
jobuuu 5:a4319f79457b 151 ADC3->CR2 |= 0x40000000; // adc _ 12bit
jobuuu 2:a1c0a37df760 152 // a1=ADC1->DR;
jobuuu 2:a1c0a37df760 153 // a1=ADC2->DR;
jobuuu 5:a4319f79457b 154 // int raw_cur = ADC3->DR;
jobuuu 5:a4319f79457b 155 while((ADC3->SR & 0b10));
jobuuu 5:a4319f79457b 156
jobuuu 5:a4319f79457b 157 double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz
jobuuu 5:a4319f79457b 158 double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA
jobuuu 7:e9086c72bb22 159 cur.sen=cur.sen*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur);
jobuuu 7:e9086c72bb22 160
jobuuu 2:a1c0a37df760 161 }
jobuuu 1:e04e563be5ce 162
jobuuu 1:e04e563be5ce 163 //DAC
jobuuu 1:e04e563be5ce 164 // dac_1 = ADC1->DR;
jobuuu 1:e04e563be5ce 165 // dac_2 = ADC2->DR;
jobuuu 1:e04e563be5ce 166
jobuuu 1:e04e563be5ce 167 /*******************************************************
jobuuu 6:df07d3491e3a 168 *** Valve Control
jobuuu 1:e04e563be5ce 169 ********************************************************/
jobuuu 7:e9086c72bb22 170 ValveControl(CONTROL_MODE);
jobuuu 1:e04e563be5ce 171
jobuuu 7:e9086c72bb22 172 double t = (double)CNT_TMR4*DT_TMR4;
jobuuu 7:e9086c72bb22 173 double T = 5.0;
jobuuu 7:e9086c72bb22 174 V_out = 1000.0*sin(2.0*PI*t/T); // V_out : -5000.0mV~5000.0mV(full duty)
jobuuu 7:e9086c72bb22 175 // if(V_out > 0.0) V_out = 1000.0;
jobuuu 7:e9086c72bb22 176 // else if(V_out < 0.0) V_out = -1000.0;
jobuuu 5:a4319f79457b 177
jobuuu 7:e9086c72bb22 178 /*******************************************************
jobuuu 7:e9086c72bb22 179 *** PWM
jobuuu 7:e9086c72bb22 180 ********************************************************/
jobuuu 5:a4319f79457b 181 PWM_out= V_out/12000.0; // Full duty : 12000.0mV
jobuuu 2:a1c0a37df760 182
jobuuu 2:a1c0a37df760 183 // Saturation of output voltage to 5.0V
jobuuu 5:a4319f79457b 184 if(PWM_out > 0.41667) PWM_out=0.41667; //5.0/12.0 = 0.41667
jobuuu 5:a4319f79457b 185 else if (PWM_out < -0.41667) PWM_out=-0.41667;
jobuuu 2:a1c0a37df760 186
jobuuu 2:a1c0a37df760 187 if (PWM_out>0.0) {
jobuuu 2:a1c0a37df760 188 dtc_v=0.0;
jobuuu 1:e04e563be5ce 189 dtc_w=PWM_out;
jobuuu 1:e04e563be5ce 190 } else {
jobuuu 2:a1c0a37df760 191 dtc_v=-PWM_out;
jobuuu 2:a1c0a37df760 192 dtc_w=0.0;
jobuuu 1:e04e563be5ce 193 }
jobuuu 1:e04e563be5ce 194
jobuuu 1:e04e563be5ce 195 //pwm
jobuuu 2:a1c0a37df760 196 TIM4->CCR2 = (PWM_ARR)*(1.0-dtc_v);
jobuuu 2:a1c0a37df760 197 TIM4->CCR1 = (PWM_ARR)*(1.0-dtc_w);
jobuuu 1:e04e563be5ce 198
jobuuu 1:e04e563be5ce 199 /*******************************************************
jobuuu 2:a1c0a37df760 200 *** Data Send (CAN) & Print out (UART)
jobuuu 1:e04e563be5ce 201 ********************************************************/
jobuuu 2:a1c0a37df760 202 if((CNT_TMR4%40)==0){
jobuuu 2:a1c0a37df760 203 msg.id = 50;
jobuuu 2:a1c0a37df760 204 msg.len = 4;
jobuuu 7:e9086c72bb22 205 int temp_CUR = (int)(cur.sen*1000.0);
jobuuu 2:a1c0a37df760 206 msg.data[0]=0x00FF&temp_CUR;
jobuuu 2:a1c0a37df760 207 msg.data[1]=0x00FF&(temp_CUR>>8);
jobuuu 5:a4319f79457b 208 int temp_PWM = (int)(V_out);
jobuuu 2:a1c0a37df760 209 msg.data[2]=0x00FF&temp_PWM;
jobuuu 2:a1c0a37df760 210 msg.data[3]=0x00FF&(temp_PWM>>8);
jobuuu 2:a1c0a37df760 211 can.write(msg);
jobuuu 2:a1c0a37df760 212 }
jobuuu 1:e04e563be5ce 213
jobuuu 2:a1c0a37df760 214 // if((CNT_TMR4%4000)==0){
jobuuu 2:a1c0a37df760 215 // pc.printf("%d\n",a1);
jobuuu 2:a1c0a37df760 216 // }
jobuuu 1:e04e563be5ce 217
jobuuu 1:e04e563be5ce 218 /*******************************************************
jobuuu 1:e04e563be5ce 219 *** Timer Counting & etc.
jobuuu 1:e04e563be5ce 220 ********************************************************/
jobuuu 2:a1c0a37df760 221 CNT_TMR4++;
jobuuu 1:e04e563be5ce 222 }
jobuuu 1:e04e563be5ce 223 TIM4->SR = 0x0; // reset the status register
jobuuu 7:e9086c72bb22 224 }
jobuuu 7:e9086c72bb22 225
jobuuu 7:e9086c72bb22 226
jobuuu 7:e9086c72bb22 227 /*******************************************************************************
jobuuu 7:e9086c72bb22 228 * CONTROL MODE
jobuuu 7:e9086c72bb22 229 ******************************************************************************/
jobuuu 7:e9086c72bb22 230 enum _CONTROL_MODE{
jobuuu 7:e9086c72bb22 231 //control mode
jobuuu 7:e9086c72bb22 232 MODE_NO_ACT = 0, //0
jobuuu 7:e9086c72bb22 233 MODE_VALVE_OPEN_LOOP, //1
jobuuu 7:e9086c72bb22 234 MODE_VALVE_POSITION_CONTROL, //2
jobuuu 7:e9086c72bb22 235
jobuuu 7:e9086c72bb22 236 MODE_JOINT_POSITION_TORQUE_CONTROL_PWM, //3
jobuuu 7:e9086c72bb22 237 MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION, //4
jobuuu 7:e9086c72bb22 238 MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING, //5
jobuuu 7:e9086c72bb22 239
jobuuu 7:e9086c72bb22 240 MODE_JOINT_POSITION_PRES_CONTROL_PWM, //6
jobuuu 7:e9086c72bb22 241 MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION, //7
jobuuu 7:e9086c72bb22 242 MODE_VALVE_POSITION_PRES_CONTROL_LEARNING, //8
jobuuu 7:e9086c72bb22 243
jobuuu 7:e9086c72bb22 244 MODE_TEST_CURRENT_CONTROL, //9
jobuuu 7:e9086c72bb22 245 MODE_TEST_PWM_CONTROL, //10
jobuuu 7:e9086c72bb22 246
jobuuu 7:e9086c72bb22 247 //utility
jobuuu 7:e9086c72bb22 248 MODE_TORQUE_SENSOR_NULLING = 20, //20
jobuuu 7:e9086c72bb22 249 MODE_VALVE_NULLING_AND_DEADZONE_SETTING, //21
jobuuu 7:e9086c72bb22 250 MODE_FIND_HOME, //22
jobuuu 7:e9086c72bb22 251 MODE_VALVE_GAIN_SETTING, //23
jobuuu 7:e9086c72bb22 252 MODE_PRESSURE_SENSOR_NULLING, //24
jobuuu 7:e9086c72bb22 253 MODE_PRESSURE_SENSOR_CALIB, //25
jobuuu 7:e9086c72bb22 254 MODE_ROTARY_FRICTION_TUNING, //26
jobuuu 7:e9086c72bb22 255 };
jobuuu 7:e9086c72bb22 256
jobuuu 7:e9086c72bb22 257 void ValveControl(unsigned int ControlMode)
jobuuu 7:e9086c72bb22 258 {
jobuuu 7:e9086c72bb22 259 switch (ControlMode) {
jobuuu 7:e9086c72bb22 260 case MODE_NO_ACT: // 0
jobuuu 7:e9086c72bb22 261 V_out = 0.0;
jobuuu 7:e9086c72bb22 262 break;
jobuuu 7:e9086c72bb22 263 case MODE_VALVE_OPEN_LOOP: // 1
jobuuu 7:e9086c72bb22 264 V_out = Vout.ref;
jobuuu 7:e9086c72bb22 265 break;
jobuuu 7:e9086c72bb22 266 case MODE_VALVE_POSITION_CONTROL: // 2
jobuuu 7:e9086c72bb22 267 CurrentControl();
jobuuu 7:e9086c72bb22 268 break;
jobuuu 7:e9086c72bb22 269 case MODE_JOINT_POSITION_TORQUE_CONTROL_PWM: // 3
jobuuu 7:e9086c72bb22 270 V_out = 0.0;
jobuuu 7:e9086c72bb22 271 break;
jobuuu 7:e9086c72bb22 272 case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION: // 4
jobuuu 7:e9086c72bb22 273 double I_REF_POS_FB = 0.0; // I_REF by Position Feedback
jobuuu 7:e9086c72bb22 274 double I_REF_POS_FF = 0.0; // I_REF by Position Feedforward
jobuuu 7:e9086c72bb22 275
jobuuu 7:e9086c72bb22 276 // feedback input for position control
jobuuu 7:e9086c72bb22 277 pos.err = pos.ref - pos.sen;
jobuuu 7:e9086c72bb22 278 double alpha_update_vel = 1.0/(1.0+(double)FREQ_TMR4/(2.0*3.1415*50.0)); // f_cutoff : 50Hz
jobuuu 7:e9086c72bb22 279 double err_diff = (pos.err - pos.err_old)*(double)FREQ_5k;
jobuuu 7:e9086c72bb22 280 pos.err_diff = (1.0-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff;
jobuuu 7:e9086c72bb22 281 pos.err_old = pos.err;
jobuuu 7:e9086c72bb22 282 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 283
jobuuu 7:e9086c72bb22 284 // feedforward input for position control
jobuuu 7:e9086c72bb22 285 double Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
jobuuu 7:e9086c72bb22 286 double K_ff = 1.3;
jobuuu 7:e9086c72bb22 287 double K_v = 0.0;
jobuuu 7:e9086c72bb22 288 if(Vel_Act_Ref > 0) K_v = 1.0/100.0; // open, tuning. (deg/s >> mA)
jobuuu 7:e9086c72bb22 289 if(Vel_Act_Ref < 0) K_v = 1.0/100.0; // close, tuning. (deg/s >> mA)
jobuuu 7:e9086c72bb22 290 I_REF_POS_FF = K_ff*K_v*Vel_Act_Ref;
jobuuu 7:e9086c72bb22 291
jobuuu 7:e9086c72bb22 292 cur.ref = I_REF_POS_FF + I_REF_POS_FB;
jobuuu 7:e9086c72bb22 293 break;
jobuuu 7:e9086c72bb22 294 case MODE_TEST_CURRENT_CONTROL: // 9
jobuuu 7:e9086c72bb22 295 V_out = 0.0;
jobuuu 7:e9086c72bb22 296 break;
jobuuu 7:e9086c72bb22 297 case MODE_TEST_PWM_CONTROL: // 10
jobuuu 7:e9086c72bb22 298 V_out = 0.0;
jobuuu 7:e9086c72bb22 299 break;
jobuuu 7:e9086c72bb22 300 case MODE_FIND_HOME: // 22
jobuuu 7:e9086c72bb22 301 V_out = 0.0;
jobuuu 7:e9086c72bb22 302 break;
jobuuu 7:e9086c72bb22 303 default:
jobuuu 7:e9086c72bb22 304 V_out = 0.0;
jobuuu 7:e9086c72bb22 305 break;
jobuuu 7:e9086c72bb22 306
jobuuu 7:e9086c72bb22 307 }
jobuuu 7:e9086c72bb22 308 }
jobuuu 7:e9086c72bb22 309
jobuuu 7:e9086c72bb22 310 void CurrentControl() {
jobuuu 7:e9086c72bb22 311 cur.err = cur.ref - cur.sen;
jobuuu 7:e9086c72bb22 312 cur.err_int = cur.err_int + cur.err*DT_TMR4;
jobuuu 7:e9086c72bb22 313 cur.err_diff = (cur.err - cur.err_old)*FREQ_TMR4;
jobuuu 7:e9086c72bb22 314 cur.err_old = cur.err;
jobuuu 7:e9086c72bb22 315
jobuuu 7:e9086c72bb22 316 double R_model = 150.0; // ohm
jobuuu 7:e9086c72bb22 317 double L_model = 0.3;
jobuuu 7:e9086c72bb22 318 double w0 = 2.0*3.14*90.0;
jobuuu 7:e9086c72bb22 319 double KP_I = L_model*w0;
jobuuu 7:e9086c72bb22 320 double KI_I = R_model*w0;
jobuuu 7:e9086c72bb22 321 double KD_I = 0.0;
jobuuu 7:e9086c72bb22 322
jobuuu 7:e9086c72bb22 323 double FF_gain = 0.0;
jobuuu 7:e9086c72bb22 324 V_out = (int) (KP_I * cur.err + KI_I * cur.err_int + KD_I * cur.err_diff);
jobuuu 7:e9086c72bb22 325 // V_out = V_out + FF_gain * (R_model*I_REF); // Unit : mV
jobuuu 7:e9086c72bb22 326 V_out = V_out + FF_gain * (R_model*cur.ref + L_model*cur.ref_diff); // Unit : mV
jobuuu 7:e9086c72bb22 327
jobuuu 7:e9086c72bb22 328 double Ka = 5.0/KP_I;
jobuuu 7:e9086c72bb22 329 if(V_out > V_MAX) {
jobuuu 7:e9086c72bb22 330 V_rem = V_out-V_MAX;
jobuuu 7:e9086c72bb22 331 V_rem = Ka*V_rem;
jobuuu 7:e9086c72bb22 332 V_out = V_MAX;
jobuuu 7:e9086c72bb22 333 cur.err_int = cur.err_int - V_rem*DT_5k;
jobuuu 7:e9086c72bb22 334 } else if(V_out < -V_MAX) {
jobuuu 7:e9086c72bb22 335 V_rem = V_out-(-V_MAX);
jobuuu 7:e9086c72bb22 336 V_rem = Ka*V_rem;
jobuuu 7:e9086c72bb22 337 V_out = -V_MAX;
jobuuu 7:e9086c72bb22 338 cur.err_int = cur.err_int - V_rem*DT_5k;
jobuuu 7:e9086c72bb22 339 }
GiJeongKim 0:51c43836c1d7 340 }