Kim GiJeong / Mbed 2 deprecated HydraulicControlBoard_LIGHT_GJ

eeprom_test

Dependencies:   mbed FastPWM

main.cpp@11:82d8768d7351, 2019-08-23 (annotated)

Committer:
Lightvalve
Date:
Fri Aug 23 12:28:08 2019 +0000
Revision:
11:82d8768d7351
Parent:
10:83a6baa77a2e
Child:
12:6f2531038ea4
190823

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