Kim GiJeong / Mbed 2 deprecated HydraulicControlBoard_LIGHT_GJ

eeprom_test

Dependencies:   mbed FastPWM

main.cpp@12:6f2531038ea4, 2019-08-27 (annotated)

Committer:
Lightvalve
Date:
Tue Aug 27 05:32:44 2019 +0000
Revision:
12:6f2531038ea4
Parent:
11:82d8768d7351
Child:
13:747daba9cf59
190827

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};
Lightvalve 12:6f2531038ea4 59
Lightvalve 12:6f2531038ea4 60 double CUR_PRES_DIFF_BAR = 0;
Lightvalve 12:6f2531038ea4 61
Lightvalve 12:6f2531038ea4 62 double CUR_PRES_A_sum = 0.0;
Lightvalve 12:6f2531038ea4 63 double CUR_PRES_B_sum = 0.0;
Lightvalve 12:6f2531038ea4 64 double CUR_PRES_A_mean = 0.0;
Lightvalve 12:6f2531038ea4 65 double CUR_PRES_B_mean = 0.0;
Lightvalve 12:6f2531038ea4 66 double CUR_TORQUE_sum = 0.0;
Lightvalve 12:6f2531038ea4 67 double CUR_TORQUE_mean = 0.0;
Lightvalve 12:6f2531038ea4 68 double PRES_A_NULL = 200.0;
Lightvalve 12:6f2531038ea4 69 double PRES_B_NULL = 200.0;
Lightvalve 12:6f2531038ea4 70 double TORQUE_NULL = 3900;
jobuuu 7:e9086c72bb22 71 // =============================================================================
jobuuu 7:e9086c72bb22 72 // =============================================================================
jobuuu 7:e9086c72bb22 73 // =============================================================================
jobuuu 7:e9086c72bb22 74
Lightvalve 12:6f2531038ea4 75 /*******************************************************************************
Lightvalve 12:6f2531038ea4 76 * REFERENCE MODE
Lightvalve 12:6f2531038ea4 77 ******************************************************************************/
Lightvalve 12:6f2531038ea4 78 enum _REFERENCE_MODE{
Lightvalve 12:6f2531038ea4 79 MODE_REF_NO_ACT = 0, //0
Lightvalve 12:6f2531038ea4 80 MODE_REF_DIRECT, //1
Lightvalve 12:6f2531038ea4 81 MODE_REF_COS_INC, //2
Lightvalve 12:6f2531038ea4 82 MODE_REF_LINE_INC, //3
Lightvalve 12:6f2531038ea4 83 MODE_REF_SIN_WAVE, //4
Lightvalve 12:6f2531038ea4 84 MODE_REF_SQUARE_WAVE, //5
Lightvalve 12:6f2531038ea4 85 };
Lightvalve 12:6f2531038ea4 86
Lightvalve 12:6f2531038ea4 87 /*******************************************************************************
Lightvalve 12:6f2531038ea4 88 * CONTROL MODE
Lightvalve 12:6f2531038ea4 89 ******************************************************************************/
Lightvalve 12:6f2531038ea4 90 enum _CONTROL_MODE{
Lightvalve 12:6f2531038ea4 91 //control mode
Lightvalve 12:6f2531038ea4 92 MODE_NO_ACT = 0, //0
Lightvalve 12:6f2531038ea4 93 MODE_VALVE_OPEN_LOOP, //1
Lightvalve 12:6f2531038ea4 94 MODE_VALVE_POSITION_CONTROL, //2
Lightvalve 12:6f2531038ea4 95
Lightvalve 12:6f2531038ea4 96 MODE_JOINT_POSITION_TORQUE_CONTROL_PWM, //3
Lightvalve 12:6f2531038ea4 97 MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION, //4
Lightvalve 12:6f2531038ea4 98 MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING, //5
Lightvalve 12:6f2531038ea4 99
Lightvalve 12:6f2531038ea4 100 MODE_JOINT_POSITION_PRES_CONTROL_PWM, //6
Lightvalve 12:6f2531038ea4 101 MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION, //7
Lightvalve 12:6f2531038ea4 102 MODE_VALVE_POSITION_PRES_CONTROL_LEARNING, //8
Lightvalve 12:6f2531038ea4 103
Lightvalve 12:6f2531038ea4 104 MODE_TEST_CURRENT_CONTROL, //9
Lightvalve 12:6f2531038ea4 105 MODE_TEST_PWM_CONTROL, //10
Lightvalve 12:6f2531038ea4 106
Lightvalve 12:6f2531038ea4 107 //utility
Lightvalve 12:6f2531038ea4 108 MODE_TORQUE_SENSOR_NULLING = 20, //20
Lightvalve 12:6f2531038ea4 109 MODE_VALVE_NULLING_AND_DEADZONE_SETTING, //21
Lightvalve 12:6f2531038ea4 110 MODE_FIND_HOME, //22
Lightvalve 12:6f2531038ea4 111 MODE_VALVE_GAIN_SETTING, //23
Lightvalve 12:6f2531038ea4 112 MODE_PRESSURE_SENSOR_NULLING, //24
Lightvalve 12:6f2531038ea4 113 MODE_PRESSURE_SENSOR_CALIB, //25
Lightvalve 12:6f2531038ea4 114 MODE_ROTARY_FRICTION_TUNING, //26
Lightvalve 12:6f2531038ea4 115
Lightvalve 12:6f2531038ea4 116 MODE_DDV_POS_VS_PWM_ID = 30, //30
Lightvalve 12:6f2531038ea4 117 MODE_DDV_DEADZONE_AND_CENTER, //31
Lightvalve 12:6f2531038ea4 118 MODE_DDV_POS_VS_FLOWRATE, //32
Lightvalve 12:6f2531038ea4 119 };
Lightvalve 12:6f2531038ea4 120
GiJeongKim 0:51c43836c1d7 121 int main()
GiJeongKim 0:51c43836c1d7 122 {
jobuuu 6:df07d3491e3a 123 /*********************************
jobuuu 1:e04e563be5ce 124 *** Initialization
jobuuu 6:df07d3491e3a 125 *********************************/
jobuuu 1:e04e563be5ce 126
Lightvalve 11:82d8768d7351 127 LED = 1;
jobuuu 1:e04e563be5ce 128 pc.baud(9600);
jobuuu 2:a1c0a37df760 129
GiJeongKim 0:51c43836c1d7 130 // i2c init
Lightvalve 8:5d2eebdad025 131 i2c.frequency(400 * 1000); // 0.4 mHz
Lightvalve 8:5d2eebdad025 132 wait_ms(2); // Power Up wait
Lightvalve 8:5d2eebdad025 133 look_for_hardware_i2c(); // Hardware present
Lightvalve 8:5d2eebdad025 134 init_as5510(i2c_slave_addr1);
Lightvalve 11:82d8768d7351 135 make_delay();
jobuuu 2:a1c0a37df760 136
GiJeongKim 0:51c43836c1d7 137 // // spi init
GiJeongKim 0:51c43836c1d7 138 eeprom.format(8,3);
GiJeongKim 0:51c43836c1d7 139 eeprom.frequency(5000000); //5M
GiJeongKim 0:51c43836c1d7 140 enc.format(8,0);
GiJeongKim 0:51c43836c1d7 141 enc.frequency(5000000); //5M
Lightvalve 11:82d8768d7351 142 make_delay();
jobuuu 2:a1c0a37df760 143
GiJeongKim 0:51c43836c1d7 144 // ADC init
jobuuu 5:a4319f79457b 145 Init_ADC();
Lightvalve 11:82d8768d7351 146 make_delay();
jobuuu 2:a1c0a37df760 147
GiJeongKim 0:51c43836c1d7 148 // Pwm init
GiJeongKim 0:51c43836c1d7 149 Init_PWM();
GiJeongKim 0:51c43836c1d7 150 TIM4->CR1 ^= TIM_CR1_UDIS;
Lightvalve 11:82d8768d7351 151 make_delay();
jobuuu 2:a1c0a37df760 152
Lightvalve 11:82d8768d7351 153 // TMR3 init
Lightvalve 11:82d8768d7351 154 Init_TMR3();
Lightvalve 11:82d8768d7351 155 TIM3->CR1 ^= TIM_CR1_UDIS;
Lightvalve 11:82d8768d7351 156 make_delay();
jobuuu 2:a1c0a37df760 157
GiJeongKim 0:51c43836c1d7 158 // CAN
jobuuu 2:a1c0a37df760 159 can.attach(&CAN_RX_HANDLER);
Lightvalve 11:82d8768d7351 160 CAN_ID_INIT();
Lightvalve 11:82d8768d7351 161 make_delay();
jobuuu 2:a1c0a37df760 162
GiJeongKim 0:51c43836c1d7 163 // spi _ enc
GiJeongKim 0:51c43836c1d7 164 spi_enc_set_init();
Lightvalve 11:82d8768d7351 165 make_delay();
Lightvalve 11:82d8768d7351 166
Lightvalve 11:82d8768d7351 167 //eeprom
Lightvalve 11:82d8768d7351 168 ROM_INIT_DATA();
Lightvalve 11:82d8768d7351 169 make_delay();
Lightvalve 11:82d8768d7351 170
Lightvalve 11:82d8768d7351 171 //DAC init
Lightvalve 11:82d8768d7351 172 dac_1 = PRES_A_VREF/3.3;
Lightvalve 11:82d8768d7351 173 dac_2 = PRES_B_VREF/3.3;
Lightvalve 11:82d8768d7351 174 make_delay();
Lightvalve 11:82d8768d7351 175
Lightvalve 11:82d8768d7351 176 for (int i=0; i<100; i++){
Lightvalve 11:82d8768d7351 177 if(i%2==0)
Lightvalve 11:82d8768d7351 178 ID_index_array[i] = - i * 0.5;
Lightvalve 11:82d8768d7351 179 else
Lightvalve 11:82d8768d7351 180 ID_index_array[i] = (i+1) * 0.5;
Lightvalve 11:82d8768d7351 181 }
jobuuu 1:e04e563be5ce 182
jobuuu 1:e04e563be5ce 183
jobuuu 6:df07d3491e3a 184 /************************************
jobuuu 1:e04e563be5ce 185 *** Program is operating!
jobuuu 6:df07d3491e3a 186 *************************************/
GiJeongKim 0:51c43836c1d7 187 while(1) {
Lightvalve 9:7f07aa6ff49a 188
GiJeongKim 0:51c43836c1d7 189 //spi _ enc
Lightvalve 11:82d8768d7351 190 //int a = spi_enc_read();
Lightvalve 11:82d8768d7351 191
Lightvalve 9:7f07aa6ff49a 192 //i2c
Lightvalve 8:5d2eebdad025 193 read_field(i2c_slave_addr1);
GiJeongKim 0:51c43836c1d7 194 }
jobuuu 1:e04e563be5ce 195 }
jobuuu 1:e04e563be5ce 196
jobuuu 6:df07d3491e3a 197
jobuuu 6:df07d3491e3a 198
jobuuu 6:df07d3491e3a 199
jobuuu 2:a1c0a37df760 200 /*******************************************************************************
jobuuu 2:a1c0a37df760 201 TIMER INTERRUPT
jobuuu 2:a1c0a37df760 202 *******************************************************************************/
jobuuu 2:a1c0a37df760 203
jobuuu 2:a1c0a37df760 204 unsigned long CNT_TMR4 = 0;
jobuuu 2:a1c0a37df760 205 double FREQ_TMR4 = (double)FREQ_20k;
jobuuu 5:a4319f79457b 206 double DT_TMR4 = (double)DT_20k;
jobuuu 1:e04e563be5ce 207 extern "C" void TIM4_IRQHandler(void)
jobuuu 1:e04e563be5ce 208 {
jobuuu 2:a1c0a37df760 209 if ( TIM4->SR & TIM_SR_UIF ) {
jobuuu 1:e04e563be5ce 210 /*******************************************************
jobuuu 1:e04e563be5ce 211 *** Sensor Read & Data Handling
jobuuu 1:e04e563be5ce 212 ********************************************************/
jobuuu 1:e04e563be5ce 213
jobuuu 2:a1c0a37df760 214 if((CNT_TMR4%2)==0){
jobuuu 2:a1c0a37df760 215
jobuuu 2:a1c0a37df760 216 //ADC
jobuuu 5:a4319f79457b 217 ADC3->CR2 |= 0x40000000; // adc _ 12bit
Lightvalve 11:82d8768d7351 218 // a1=ADC1->DR;
Lightvalve 11:82d8768d7351 219 // a1=ADC2->DR;
Lightvalve 11:82d8768d7351 220 // int raw_cur = ADC3->DR;
jobuuu 5:a4319f79457b 221 while((ADC3->SR & 0b10));
jobuuu 5:a4319f79457b 222
jobuuu 5:a4319f79457b 223 double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz
jobuuu 5:a4319f79457b 224 double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA
jobuuu 7:e9086c72bb22 225 cur.sen=cur.sen*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur);
jobuuu 2:a1c0a37df760 226 }
jobuuu 1:e04e563be5ce 227
jobuuu 1:e04e563be5ce 228 //DAC
jobuuu 1:e04e563be5ce 229 // dac_1 = ADC1->DR;
jobuuu 1:e04e563be5ce 230 // dac_2 = ADC2->DR;
Lightvalve 11:82d8768d7351 231
Lightvalve 11:82d8768d7351 232
Lightvalve 11:82d8768d7351 233
Lightvalve 11:82d8768d7351 234 /*******************************************************
Lightvalve 11:82d8768d7351 235 *** Timer Counting & etc.
Lightvalve 11:82d8768d7351 236 ********************************************************/
Lightvalve 11:82d8768d7351 237 CNT_TMR4++;
Lightvalve 11:82d8768d7351 238 }
Lightvalve 11:82d8768d7351 239 TIM4->SR = 0x0; // reset the status register
Lightvalve 11:82d8768d7351 240 }
Lightvalve 11:82d8768d7351 241
Lightvalve 11:82d8768d7351 242 unsigned long CNT_TMR3 = 0;
Lightvalve 11:82d8768d7351 243 double FREQ_TMR3 = (double)FREQ_5k;
Lightvalve 11:82d8768d7351 244 double DT_TMR3 = (double)DT_5k;
Lightvalve 11:82d8768d7351 245 extern "C" void TIM3_IRQHandler(void)
Lightvalve 11:82d8768d7351 246 {
Lightvalve 11:82d8768d7351 247 if ( TIM3->SR & TIM_SR_UIF ) {
jobuuu 1:e04e563be5ce 248
Lightvalve 12:6f2531038ea4 249 ENC_UPDATE();
Lightvalve 12:6f2531038ea4 250 CUR_PRES_A_BAR = (CUR_PRES_A - PRES_A_NULL) / PRES_SENSOR_A_PULSE_PER_BAR + 1.;
Lightvalve 12:6f2531038ea4 251 CUR_PRES_B_BAR = (CUR_PRES_B - PRES_B_NULL) / PRES_SENSOR_B_PULSE_PER_BAR + 1.;
Lightvalve 12:6f2531038ea4 252 //CUR_TORQUE_NM = (CUR_TORQUE - TORQUE_NULL) / (double) TORQUE_SENSOR_PULSE_PER_TORQUE; //
Lightvalve 12:6f2531038ea4 253 CUR_TORQUE_NM = CUR_TORQUE; //
Lightvalve 12:6f2531038ea4 254 CUR_TORQUE_NM_PRESS = 1. * (CUR_PRES_A_BAR - CUR_PRES_B_BAR);
Lightvalve 12:6f2531038ea4 255
Lightvalve 12:6f2531038ea4 256 // Reference Loop
Lightvalve 12:6f2531038ea4 257 switch (REFERENCE_MODE) {
Lightvalve 12:6f2531038ea4 258 case MODE_REF_NO_ACT:
Lightvalve 12:6f2531038ea4 259 {
Lightvalve 12:6f2531038ea4 260 break;
Lightvalve 12:6f2531038ea4 261 }
Lightvalve 12:6f2531038ea4 262
Lightvalve 12:6f2531038ea4 263 case MODE_REF_DIRECT:
Lightvalve 12:6f2531038ea4 264 {
Lightvalve 12:6f2531038ea4 265 if (FLAG_REFERENCE_VALVE_PWM) {
Lightvalve 12:6f2531038ea4 266 Ref_PWM = (double) REF_PWM;
Lightvalve 12:6f2531038ea4 267 }
Lightvalve 12:6f2531038ea4 268 if (FLAG_REFERENCE_VALVE_POSITION) {
Lightvalve 12:6f2531038ea4 269 Ref_Valve_Pos = (double) REF_VALVE_POSITION;
Lightvalve 12:6f2531038ea4 270 }
Lightvalve 12:6f2531038ea4 271 if (FLAG_REFERENCE_JOINT_POSITION) {
Lightvalve 12:6f2531038ea4 272 Ref_Joint_Pos = (double) REF_POSITION;
Lightvalve 12:6f2531038ea4 273 Ref_Joint_Vel = (double) REF_VELOCITY;
Lightvalve 12:6f2531038ea4 274 }
Lightvalve 12:6f2531038ea4 275 if (FLAG_REFERENCE_JOINT_TORQUE) {
Lightvalve 12:6f2531038ea4 276 Ref_Joint_Torq = (double) REF_TORQUE;
Lightvalve 12:6f2531038ea4 277 }
Lightvalve 12:6f2531038ea4 278 break;
Lightvalve 12:6f2531038ea4 279 }
Lightvalve 12:6f2531038ea4 280
Lightvalve 12:6f2531038ea4 281 case MODE_REF_COS_INC:
Lightvalve 12:6f2531038ea4 282 {
Lightvalve 12:6f2531038ea4 283 if (FLAG_REFERENCE_VALVE_PWM) {
Lightvalve 12:6f2531038ea4 284 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;
Lightvalve 12:6f2531038ea4 285 }
Lightvalve 12:6f2531038ea4 286 if (FLAG_REFERENCE_VALVE_POSITION) {
Lightvalve 12:6f2531038ea4 287 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;
Lightvalve 12:6f2531038ea4 288 }
Lightvalve 12:6f2531038ea4 289 if (FLAG_REFERENCE_JOINT_POSITION) {
Lightvalve 12:6f2531038ea4 290 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;
Lightvalve 12:6f2531038ea4 291 Ref_Joint_Vel = 0.0;
Lightvalve 12:6f2531038ea4 292 }
Lightvalve 12:6f2531038ea4 293 if (FLAG_REFERENCE_JOINT_TORQUE) {
Lightvalve 12:6f2531038ea4 294 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;
Lightvalve 12:6f2531038ea4 295 }
Lightvalve 12:6f2531038ea4 296 TMR2_COUNT_REFERENCE++;
Lightvalve 12:6f2531038ea4 297 if (TMR2_COUNT_REFERENCE >= REF_MOVE_TIME_5k) {
Lightvalve 12:6f2531038ea4 298 REFERENCE_MODE = MODE_REF_DIRECT;
Lightvalve 12:6f2531038ea4 299 TMR2_COUNT_REFERENCE = 0;
Lightvalve 12:6f2531038ea4 300 }
Lightvalve 12:6f2531038ea4 301 break;
Lightvalve 12:6f2531038ea4 302 }
Lightvalve 12:6f2531038ea4 303
Lightvalve 12:6f2531038ea4 304
Lightvalve 12:6f2531038ea4 305 case MODE_REF_LINE_INC:
Lightvalve 12:6f2531038ea4 306 {
Lightvalve 12:6f2531038ea4 307 if (FLAG_REFERENCE_VALVE_PWM) {
Lightvalve 12:6f2531038ea4 308 Ref_PWM = ((double) REF_PWM - (double) INIT_REF_PWM)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_PWM;
Lightvalve 12:6f2531038ea4 309 }
Lightvalve 12:6f2531038ea4 310 if (FLAG_REFERENCE_VALVE_POSITION) {
Lightvalve 12:6f2531038ea4 311 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;
Lightvalve 12:6f2531038ea4 312 }
Lightvalve 12:6f2531038ea4 313 if (FLAG_REFERENCE_JOINT_POSITION) {
Lightvalve 12:6f2531038ea4 314 Ref_Joint_Pos = ((double) REF_POSITION - (double) INIT_REF_POS)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_POS;
Lightvalve 12:6f2531038ea4 315 Ref_Vel_Test = ((double) REF_POSITION - (double) INIT_REF_POS) / (double) REF_MOVE_TIME_5k * (double) TMR_FREQ_5k; // pulse/sec
Lightvalve 12:6f2531038ea4 316 //Ref_Vel_Test = 10;
Lightvalve 12:6f2531038ea4 317 }
Lightvalve 12:6f2531038ea4 318 if (FLAG_REFERENCE_JOINT_TORQUE) {
Lightvalve 12:6f2531038ea4 319 Ref_Joint_Torq = ((double) REF_TORQUE - (double) INIT_REF_TORQUE)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_TORQUE;
Lightvalve 12:6f2531038ea4 320 }
Lightvalve 12:6f2531038ea4 321 TMR2_COUNT_REFERENCE++;
Lightvalve 12:6f2531038ea4 322 if (TMR2_COUNT_REFERENCE >= REF_MOVE_TIME_5k) {
Lightvalve 12:6f2531038ea4 323 //REFERENCE_MODE = MODE_REF_DIRECT;
Lightvalve 12:6f2531038ea4 324 Ref_Vel_Test = 1;
Lightvalve 12:6f2531038ea4 325 TMR2_COUNT_REFERENCE = REF_MOVE_TIME_5k;
Lightvalve 12:6f2531038ea4 326 //TMR2_COUNT_REFERENCE = 0;
Lightvalve 12:6f2531038ea4 327 }
Lightvalve 12:6f2531038ea4 328 break;
Lightvalve 12:6f2531038ea4 329 }
Lightvalve 12:6f2531038ea4 330
Lightvalve 12:6f2531038ea4 331 case MODE_REF_SIN_WAVE:
Lightvalve 12:6f2531038ea4 332 {
Lightvalve 12:6f2531038ea4 333 if (FLAG_REFERENCE_VALVE_PWM) {
Lightvalve 12:6f2531038ea4 334 Ref_PWM = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_PWM;
Lightvalve 12:6f2531038ea4 335 }
Lightvalve 12:6f2531038ea4 336 if (FLAG_REFERENCE_VALVE_POSITION) {
Lightvalve 12:6f2531038ea4 337 Ref_Valve_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_VALVE_POSITION;
Lightvalve 12:6f2531038ea4 338 }
Lightvalve 12:6f2531038ea4 339 if (FLAG_REFERENCE_JOINT_POSITION) {
Lightvalve 12:6f2531038ea4 340 Ref_Joint_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_POSITION;
Lightvalve 12:6f2531038ea4 341 }
Lightvalve 12:6f2531038ea4 342 if (FLAG_REFERENCE_JOINT_TORQUE) {
Lightvalve 12:6f2531038ea4 343 Ref_Joint_Torq = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_TORQUE;
Lightvalve 12:6f2531038ea4 344 }
Lightvalve 12:6f2531038ea4 345 TMR2_COUNT_REFERENCE++;
Lightvalve 12:6f2531038ea4 346 if (TMR2_COUNT_REFERENCE >= TMR2_COUNT_REFERENCE * REF_NUM) {
Lightvalve 12:6f2531038ea4 347 REFERENCE_MODE = MODE_REF_DIRECT;
Lightvalve 12:6f2531038ea4 348 TMR2_COUNT_REFERENCE = 0;
Lightvalve 12:6f2531038ea4 349 }
Lightvalve 12:6f2531038ea4 350 break;
Lightvalve 12:6f2531038ea4 351 }
Lightvalve 12:6f2531038ea4 352
Lightvalve 12:6f2531038ea4 353 case MODE_REF_SQUARE_WAVE:
Lightvalve 12:6f2531038ea4 354 {
Lightvalve 12:6f2531038ea4 355 if (FLAG_REFERENCE_VALVE_PWM) {
Lightvalve 12:6f2531038ea4 356 Ref_PWM = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_PWM;
Lightvalve 12:6f2531038ea4 357 if (Ref_PWM >= REF_PWM) Ref_PWM = REF_MAG + REF_PWM;
Lightvalve 12:6f2531038ea4 358 else Ref_PWM = -REF_MAG + REF_PWM;
Lightvalve 12:6f2531038ea4 359 }
Lightvalve 12:6f2531038ea4 360 if (FLAG_REFERENCE_VALVE_POSITION) {
Lightvalve 12:6f2531038ea4 361 Ref_Valve_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_VALVE_POSITION;
Lightvalve 12:6f2531038ea4 362 if (Ref_Valve_Pos >= REF_VALVE_POSITION) Ref_Valve_Pos = REF_MAG + REF_VALVE_POSITION;
Lightvalve 12:6f2531038ea4 363 else Ref_Valve_Pos = -REF_MAG + REF_VALVE_POSITION;
Lightvalve 12:6f2531038ea4 364 }
Lightvalve 12:6f2531038ea4 365 if (FLAG_REFERENCE_JOINT_POSITION) {
Lightvalve 12:6f2531038ea4 366 Ref_Joint_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_POSITION;
Lightvalve 12:6f2531038ea4 367 if (Ref_Joint_Pos >= REF_POSITION) Ref_Valve_Pos = REF_MAG + REF_POSITION;
Lightvalve 12:6f2531038ea4 368 else Ref_Joint_Pos = -REF_MAG + REF_POSITION;
Lightvalve 12:6f2531038ea4 369 }
Lightvalve 12:6f2531038ea4 370 if (FLAG_REFERENCE_JOINT_TORQUE) {
Lightvalve 12:6f2531038ea4 371 Ref_Joint_Torq = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_TORQUE;
Lightvalve 12:6f2531038ea4 372 if (Ref_Joint_Torq >= REF_TORQUE) Ref_Valve_Pos = REF_MAG + REF_TORQUE;
Lightvalve 12:6f2531038ea4 373 else Ref_Joint_Torq = -REF_MAG + REF_TORQUE;
Lightvalve 12:6f2531038ea4 374 }
Lightvalve 12:6f2531038ea4 375 TMR2_COUNT_REFERENCE++;
Lightvalve 12:6f2531038ea4 376 if (TMR2_COUNT_REFERENCE >= TMR2_COUNT_REFERENCE * REF_NUM) {
Lightvalve 12:6f2531038ea4 377 REFERENCE_MODE = MODE_REF_DIRECT;
Lightvalve 12:6f2531038ea4 378 TMR2_COUNT_REFERENCE = 0;
Lightvalve 12:6f2531038ea4 379 }
Lightvalve 12:6f2531038ea4 380 break;
Lightvalve 12:6f2531038ea4 381 }
Lightvalve 12:6f2531038ea4 382
Lightvalve 12:6f2531038ea4 383 default:
Lightvalve 12:6f2531038ea4 384 break;
Lightvalve 12:6f2531038ea4 385 }
Lightvalve 12:6f2531038ea4 386
Lightvalve 12:6f2531038ea4 387
Lightvalve 12:6f2531038ea4 388
jobuuu 1:e04e563be5ce 389 /*******************************************************
jobuuu 6:df07d3491e3a 390 *** Valve Control
jobuuu 1:e04e563be5ce 391 ********************************************************/
jobuuu 7:e9086c72bb22 392 ValveControl(CONTROL_MODE);
jobuuu 1:e04e563be5ce 393
jobuuu 7:e9086c72bb22 394 double t = (double)CNT_TMR4*DT_TMR4;
jobuuu 7:e9086c72bb22 395 double T = 5.0;
jobuuu 7:e9086c72bb22 396 V_out = 1000.0*sin(2.0*PI*t/T); // V_out : -5000.0mV~5000.0mV(full duty)
jobuuu 7:e9086c72bb22 397 // if(V_out > 0.0) V_out = 1000.0;
jobuuu 7:e9086c72bb22 398 // else if(V_out < 0.0) V_out = -1000.0;
jobuuu 5:a4319f79457b 399
jobuuu 7:e9086c72bb22 400 /*******************************************************
jobuuu 7:e9086c72bb22 401 *** PWM
jobuuu 7:e9086c72bb22 402 ********************************************************/
jobuuu 5:a4319f79457b 403 PWM_out= V_out/12000.0; // Full duty : 12000.0mV
jobuuu 2:a1c0a37df760 404
jobuuu 2:a1c0a37df760 405 // Saturation of output voltage to 5.0V
jobuuu 5:a4319f79457b 406 if(PWM_out > 0.41667) PWM_out=0.41667; //5.0/12.0 = 0.41667
jobuuu 5:a4319f79457b 407 else if (PWM_out < -0.41667) PWM_out=-0.41667;
jobuuu 2:a1c0a37df760 408
jobuuu 2:a1c0a37df760 409 if (PWM_out>0.0) {
jobuuu 2:a1c0a37df760 410 dtc_v=0.0;
jobuuu 1:e04e563be5ce 411 dtc_w=PWM_out;
jobuuu 1:e04e563be5ce 412 } else {
jobuuu 2:a1c0a37df760 413 dtc_v=-PWM_out;
jobuuu 2:a1c0a37df760 414 dtc_w=0.0;
jobuuu 1:e04e563be5ce 415 }
jobuuu 1:e04e563be5ce 416
jobuuu 1:e04e563be5ce 417 //pwm
jobuuu 2:a1c0a37df760 418 TIM4->CCR2 = (PWM_ARR)*(1.0-dtc_v);
jobuuu 2:a1c0a37df760 419 TIM4->CCR1 = (PWM_ARR)*(1.0-dtc_w);
jobuuu 1:e04e563be5ce 420
jobuuu 1:e04e563be5ce 421 /*******************************************************
jobuuu 2:a1c0a37df760 422 *** Data Send (CAN) & Print out (UART)
jobuuu 1:e04e563be5ce 423 ********************************************************/
Lightvalve 11:82d8768d7351 424 //if((CNT_TMR3%40)==0){
Lightvalve 11:82d8768d7351 425 // msg.id = 50;
Lightvalve 11:82d8768d7351 426 // msg.len = 4;
Lightvalve 11:82d8768d7351 427 // int temp_CUR = (int)(cur.sen*1000.0);
Lightvalve 11:82d8768d7351 428 // msg.data[0]=0x00FF&temp_CUR;
Lightvalve 11:82d8768d7351 429 // msg.data[1]=0x00FF&(temp_CUR>>8);
Lightvalve 11:82d8768d7351 430 // int temp_PWM = (int)(V_out);
Lightvalve 11:82d8768d7351 431 // msg.data[2]=0x00FF&temp_PWM;
Lightvalve 11:82d8768d7351 432 // msg.data[3]=0x00FF&(temp_PWM>>8);
Lightvalve 11:82d8768d7351 433 // can.write(msg);
Lightvalve 11:82d8768d7351 434 // }
Lightvalve 11:82d8768d7351 435
Lightvalve 11:82d8768d7351 436 if((CNT_TMR3%5000)==0){
Lightvalve 11:82d8768d7351 437 if(LED==1)
Lightvalve 11:82d8768d7351 438 {
Lightvalve 11:82d8768d7351 439 LED=0;
Lightvalve 11:82d8768d7351 440 }
Lightvalve 11:82d8768d7351 441 else
Lightvalve 11:82d8768d7351 442 LED = 1;
Lightvalve 11:82d8768d7351 443 // LED != LED;
jobuuu 2:a1c0a37df760 444 }
jobuuu 1:e04e563be5ce 445
jobuuu 1:e04e563be5ce 446 /*******************************************************
jobuuu 1:e04e563be5ce 447 *** Timer Counting & etc.
jobuuu 1:e04e563be5ce 448 ********************************************************/
Lightvalve 11:82d8768d7351 449 CNT_TMR3++;
jobuuu 1:e04e563be5ce 450 }
Lightvalve 11:82d8768d7351 451 TIM3->SR = 0x0; // reset the status register
jobuuu 7:e9086c72bb22 452 }
jobuuu 7:e9086c72bb22 453
jobuuu 7:e9086c72bb22 454 void ValveControl(unsigned int ControlMode)
jobuuu 7:e9086c72bb22 455 {
jobuuu 7:e9086c72bb22 456 switch (ControlMode) {
jobuuu 7:e9086c72bb22 457 case MODE_NO_ACT: // 0
jobuuu 7:e9086c72bb22 458 V_out = 0.0;
jobuuu 7:e9086c72bb22 459 break;
jobuuu 7:e9086c72bb22 460 case MODE_VALVE_OPEN_LOOP: // 1
jobuuu 7:e9086c72bb22 461 V_out = Vout.ref;
jobuuu 7:e9086c72bb22 462 break;
jobuuu 7:e9086c72bb22 463 case MODE_VALVE_POSITION_CONTROL: // 2
jobuuu 7:e9086c72bb22 464 CurrentControl();
jobuuu 7:e9086c72bb22 465 break;
jobuuu 7:e9086c72bb22 466 case MODE_JOINT_POSITION_TORQUE_CONTROL_PWM: // 3
jobuuu 7:e9086c72bb22 467 V_out = 0.0;
jobuuu 7:e9086c72bb22 468 break;
jobuuu 7:e9086c72bb22 469 case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION: // 4
jobuuu 7:e9086c72bb22 470 double I_REF_POS_FB = 0.0; // I_REF by Position Feedback
jobuuu 7:e9086c72bb22 471 double I_REF_POS_FF = 0.0; // I_REF by Position Feedforward
jobuuu 7:e9086c72bb22 472
jobuuu 7:e9086c72bb22 473 // feedback input for position control
jobuuu 7:e9086c72bb22 474 pos.err = pos.ref - pos.sen;
jobuuu 7:e9086c72bb22 475 double alpha_update_vel = 1.0/(1.0+(double)FREQ_TMR4/(2.0*3.1415*50.0)); // f_cutoff : 50Hz
jobuuu 7:e9086c72bb22 476 double err_diff = (pos.err - pos.err_old)*(double)FREQ_5k;
jobuuu 7:e9086c72bb22 477 pos.err_diff = (1.0-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff;
jobuuu 7:e9086c72bb22 478 pos.err_old = pos.err;
jobuuu 7:e9086c72bb22 479 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 480
jobuuu 7:e9086c72bb22 481 // feedforward input for position control
jobuuu 7:e9086c72bb22 482 double Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
jobuuu 7:e9086c72bb22 483 double K_ff = 1.3;
jobuuu 7:e9086c72bb22 484 double K_v = 0.0;
jobuuu 7:e9086c72bb22 485 if(Vel_Act_Ref > 0) K_v = 1.0/100.0; // open, tuning. (deg/s >> mA)
jobuuu 7:e9086c72bb22 486 if(Vel_Act_Ref < 0) K_v = 1.0/100.0; // close, tuning. (deg/s >> mA)
jobuuu 7:e9086c72bb22 487 I_REF_POS_FF = K_ff*K_v*Vel_Act_Ref;
jobuuu 7:e9086c72bb22 488
jobuuu 7:e9086c72bb22 489 cur.ref = I_REF_POS_FF + I_REF_POS_FB;
jobuuu 7:e9086c72bb22 490 break;
jobuuu 7:e9086c72bb22 491 case MODE_TEST_CURRENT_CONTROL: // 9
jobuuu 7:e9086c72bb22 492 V_out = 0.0;
jobuuu 7:e9086c72bb22 493 break;
jobuuu 7:e9086c72bb22 494 case MODE_TEST_PWM_CONTROL: // 10
jobuuu 7:e9086c72bb22 495 V_out = 0.0;
jobuuu 7:e9086c72bb22 496 break;
jobuuu 7:e9086c72bb22 497 case MODE_FIND_HOME: // 22
jobuuu 7:e9086c72bb22 498 V_out = 0.0;
jobuuu 7:e9086c72bb22 499 break;
jobuuu 7:e9086c72bb22 500 default:
jobuuu 7:e9086c72bb22 501 V_out = 0.0;
jobuuu 7:e9086c72bb22 502 break;
jobuuu 7:e9086c72bb22 503
jobuuu 7:e9086c72bb22 504 }
jobuuu 7:e9086c72bb22 505 }
jobuuu 7:e9086c72bb22 506
jobuuu 7:e9086c72bb22 507 void CurrentControl() {
jobuuu 7:e9086c72bb22 508 cur.err = cur.ref - cur.sen;
jobuuu 7:e9086c72bb22 509 cur.err_int = cur.err_int + cur.err*DT_TMR4;
jobuuu 7:e9086c72bb22 510 cur.err_diff = (cur.err - cur.err_old)*FREQ_TMR4;
jobuuu 7:e9086c72bb22 511 cur.err_old = cur.err;
jobuuu 7:e9086c72bb22 512
jobuuu 7:e9086c72bb22 513 double R_model = 150.0; // ohm
jobuuu 7:e9086c72bb22 514 double L_model = 0.3;
jobuuu 7:e9086c72bb22 515 double w0 = 2.0*3.14*90.0;
jobuuu 7:e9086c72bb22 516 double KP_I = L_model*w0;
jobuuu 7:e9086c72bb22 517 double KI_I = R_model*w0;
jobuuu 7:e9086c72bb22 518 double KD_I = 0.0;
jobuuu 7:e9086c72bb22 519
jobuuu 7:e9086c72bb22 520 double FF_gain = 0.0;
jobuuu 7:e9086c72bb22 521 V_out = (int) (KP_I * cur.err + KI_I * cur.err_int + KD_I * cur.err_diff);
jobuuu 7:e9086c72bb22 522 // V_out = V_out + FF_gain * (R_model*I_REF); // Unit : mV
jobuuu 7:e9086c72bb22 523 V_out = V_out + FF_gain * (R_model*cur.ref + L_model*cur.ref_diff); // Unit : mV
jobuuu 7:e9086c72bb22 524
jobuuu 7:e9086c72bb22 525 double Ka = 5.0/KP_I;
jobuuu 7:e9086c72bb22 526 if(V_out > V_MAX) {
jobuuu 7:e9086c72bb22 527 V_rem = V_out-V_MAX;
jobuuu 7:e9086c72bb22 528 V_rem = Ka*V_rem;
jobuuu 7:e9086c72bb22 529 V_out = V_MAX;
jobuuu 7:e9086c72bb22 530 cur.err_int = cur.err_int - V_rem*DT_5k;
jobuuu 7:e9086c72bb22 531 } else if(V_out < -V_MAX) {
jobuuu 7:e9086c72bb22 532 V_rem = V_out-(-V_MAX);
jobuuu 7:e9086c72bb22 533 V_rem = Ka*V_rem;
jobuuu 7:e9086c72bb22 534 V_out = -V_MAX;
jobuuu 7:e9086c72bb22 535 cur.err_int = cur.err_int - V_rem*DT_5k;
jobuuu 7:e9086c72bb22 536 }
GiJeongKim 0:51c43836c1d7 537 }