Kim GiJeong / Mbed 2 deprecated HydraulicControlBoard_LIGHT_GJ

eeprom_test

Dependencies:   mbed FastPWM

main.cpp@13:747daba9cf59, 2019-08-27 (annotated)

Committer:
Lightvalve
Date:
Tue Aug 27 06:50:54 2019 +0000
Revision:
13:747daba9cf59
Parent:
12:6f2531038ea4
Child:
14:8e7590227d22
1908271550

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 12:6f2531038ea4 58
Lightvalve 13:747daba9cf59 59
jobuuu 7:e9086c72bb22 60 // =============================================================================
jobuuu 7:e9086c72bb22 61 // =============================================================================
jobuuu 7:e9086c72bb22 62 // =============================================================================
jobuuu 7:e9086c72bb22 63
Lightvalve 12:6f2531038ea4 64 /*******************************************************************************
Lightvalve 12:6f2531038ea4 65 * REFERENCE MODE
Lightvalve 12:6f2531038ea4 66 ******************************************************************************/
Lightvalve 13:747daba9cf59 67 enum _REFERENCE_MODE {
Lightvalve 13:747daba9cf59 68 MODE_REF_NO_ACT = 0, //0
Lightvalve 12:6f2531038ea4 69 MODE_REF_DIRECT, //1
Lightvalve 12:6f2531038ea4 70 MODE_REF_COS_INC, //2
Lightvalve 12:6f2531038ea4 71 MODE_REF_LINE_INC, //3
Lightvalve 12:6f2531038ea4 72 MODE_REF_SIN_WAVE, //4
Lightvalve 12:6f2531038ea4 73 MODE_REF_SQUARE_WAVE, //5
Lightvalve 12:6f2531038ea4 74 };
Lightvalve 12:6f2531038ea4 75
Lightvalve 12:6f2531038ea4 76 /*******************************************************************************
Lightvalve 12:6f2531038ea4 77 * CONTROL MODE
Lightvalve 12:6f2531038ea4 78 ******************************************************************************/
Lightvalve 13:747daba9cf59 79 enum _CONTROL_MODE {
Lightvalve 12:6f2531038ea4 80 //control mode
Lightvalve 12:6f2531038ea4 81 MODE_NO_ACT = 0, //0
Lightvalve 12:6f2531038ea4 82 MODE_VALVE_OPEN_LOOP, //1
Lightvalve 12:6f2531038ea4 83 MODE_VALVE_POSITION_CONTROL, //2
Lightvalve 13:747daba9cf59 84
Lightvalve 12:6f2531038ea4 85 MODE_JOINT_POSITION_TORQUE_CONTROL_PWM, //3
Lightvalve 12:6f2531038ea4 86 MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION, //4
Lightvalve 12:6f2531038ea4 87 MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING, //5
Lightvalve 13:747daba9cf59 88
Lightvalve 12:6f2531038ea4 89 MODE_JOINT_POSITION_PRES_CONTROL_PWM, //6
Lightvalve 12:6f2531038ea4 90 MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION, //7
Lightvalve 12:6f2531038ea4 91 MODE_VALVE_POSITION_PRES_CONTROL_LEARNING, //8
Lightvalve 13:747daba9cf59 92
Lightvalve 12:6f2531038ea4 93 MODE_TEST_CURRENT_CONTROL, //9
Lightvalve 12:6f2531038ea4 94 MODE_TEST_PWM_CONTROL, //10
Lightvalve 13:747daba9cf59 95
Lightvalve 12:6f2531038ea4 96 //utility
Lightvalve 12:6f2531038ea4 97 MODE_TORQUE_SENSOR_NULLING = 20, //20
Lightvalve 12:6f2531038ea4 98 MODE_VALVE_NULLING_AND_DEADZONE_SETTING, //21
Lightvalve 12:6f2531038ea4 99 MODE_FIND_HOME, //22
Lightvalve 12:6f2531038ea4 100 MODE_VALVE_GAIN_SETTING, //23
Lightvalve 12:6f2531038ea4 101 MODE_PRESSURE_SENSOR_NULLING, //24
Lightvalve 12:6f2531038ea4 102 MODE_PRESSURE_SENSOR_CALIB, //25
Lightvalve 12:6f2531038ea4 103 MODE_ROTARY_FRICTION_TUNING, //26
Lightvalve 13:747daba9cf59 104
Lightvalve 12:6f2531038ea4 105 MODE_DDV_POS_VS_PWM_ID = 30, //30
Lightvalve 12:6f2531038ea4 106 MODE_DDV_DEADZONE_AND_CENTER, //31
Lightvalve 12:6f2531038ea4 107 MODE_DDV_POS_VS_FLOWRATE, //32
Lightvalve 12:6f2531038ea4 108 };
Lightvalve 12:6f2531038ea4 109
GiJeongKim 0:51c43836c1d7 110 int main()
GiJeongKim 0:51c43836c1d7 111 {
jobuuu 6:df07d3491e3a 112 /*********************************
jobuuu 1:e04e563be5ce 113 *** Initialization
jobuuu 6:df07d3491e3a 114 *********************************/
Lightvalve 13:747daba9cf59 115
Lightvalve 11:82d8768d7351 116 LED = 1;
jobuuu 1:e04e563be5ce 117 pc.baud(9600);
jobuuu 2:a1c0a37df760 118
GiJeongKim 0:51c43836c1d7 119 // i2c init
Lightvalve 8:5d2eebdad025 120 i2c.frequency(400 * 1000); // 0.4 mHz
Lightvalve 8:5d2eebdad025 121 wait_ms(2); // Power Up wait
Lightvalve 8:5d2eebdad025 122 look_for_hardware_i2c(); // Hardware present
Lightvalve 8:5d2eebdad025 123 init_as5510(i2c_slave_addr1);
Lightvalve 11:82d8768d7351 124 make_delay();
jobuuu 2:a1c0a37df760 125
GiJeongKim 0:51c43836c1d7 126 // // spi init
GiJeongKim 0:51c43836c1d7 127 eeprom.format(8,3);
GiJeongKim 0:51c43836c1d7 128 eeprom.frequency(5000000); //5M
GiJeongKim 0:51c43836c1d7 129 enc.format(8,0);
GiJeongKim 0:51c43836c1d7 130 enc.frequency(5000000); //5M
Lightvalve 11:82d8768d7351 131 make_delay();
Lightvalve 13:747daba9cf59 132
GiJeongKim 0:51c43836c1d7 133 // ADC init
jobuuu 5:a4319f79457b 134 Init_ADC();
Lightvalve 11:82d8768d7351 135 make_delay();
jobuuu 2:a1c0a37df760 136
GiJeongKim 0:51c43836c1d7 137 // Pwm init
GiJeongKim 0:51c43836c1d7 138 Init_PWM();
GiJeongKim 0:51c43836c1d7 139 TIM4->CR1 ^= TIM_CR1_UDIS;
Lightvalve 11:82d8768d7351 140 make_delay();
Lightvalve 13:747daba9cf59 141
Lightvalve 11:82d8768d7351 142 // TMR3 init
Lightvalve 11:82d8768d7351 143 Init_TMR3();
Lightvalve 11:82d8768d7351 144 TIM3->CR1 ^= TIM_CR1_UDIS;
Lightvalve 11:82d8768d7351 145 make_delay();
jobuuu 2:a1c0a37df760 146
GiJeongKim 0:51c43836c1d7 147 // CAN
jobuuu 2:a1c0a37df760 148 can.attach(&CAN_RX_HANDLER);
Lightvalve 11:82d8768d7351 149 CAN_ID_INIT();
Lightvalve 11:82d8768d7351 150 make_delay();
Lightvalve 13:747daba9cf59 151
GiJeongKim 0:51c43836c1d7 152 // spi _ enc
GiJeongKim 0:51c43836c1d7 153 spi_enc_set_init();
Lightvalve 11:82d8768d7351 154 make_delay();
Lightvalve 13:747daba9cf59 155
Lightvalve 11:82d8768d7351 156 //eeprom
Lightvalve 11:82d8768d7351 157 ROM_INIT_DATA();
Lightvalve 11:82d8768d7351 158 make_delay();
Lightvalve 13:747daba9cf59 159
Lightvalve 11:82d8768d7351 160 //DAC init
Lightvalve 11:82d8768d7351 161 dac_1 = PRES_A_VREF/3.3;
Lightvalve 11:82d8768d7351 162 dac_2 = PRES_B_VREF/3.3;
Lightvalve 11:82d8768d7351 163 make_delay();
Lightvalve 13:747daba9cf59 164
Lightvalve 13:747daba9cf59 165 for (int i=0; i<100; i++) {
Lightvalve 11:82d8768d7351 166 if(i%2==0)
Lightvalve 11:82d8768d7351 167 ID_index_array[i] = - i * 0.5;
Lightvalve 11:82d8768d7351 168 else
Lightvalve 11:82d8768d7351 169 ID_index_array[i] = (i+1) * 0.5;
Lightvalve 11:82d8768d7351 170 }
Lightvalve 13:747daba9cf59 171
Lightvalve 13:747daba9cf59 172
jobuuu 6:df07d3491e3a 173 /************************************
jobuuu 1:e04e563be5ce 174 *** Program is operating!
jobuuu 6:df07d3491e3a 175 *************************************/
GiJeongKim 0:51c43836c1d7 176 while(1) {
Lightvalve 13:747daba9cf59 177
GiJeongKim 0:51c43836c1d7 178 //spi _ enc
Lightvalve 11:82d8768d7351 179 //int a = spi_enc_read();
Lightvalve 13:747daba9cf59 180
Lightvalve 9:7f07aa6ff49a 181 //i2c
Lightvalve 8:5d2eebdad025 182 read_field(i2c_slave_addr1);
GiJeongKim 0:51c43836c1d7 183 }
jobuuu 1:e04e563be5ce 184 }
jobuuu 1:e04e563be5ce 185
Lightvalve 13:747daba9cf59 186 int DDV_JOINT_POS_FF(double REF_JOINT_VEL){
Lightvalve 13:747daba9cf59 187
Lightvalve 13:747daba9cf59 188 int i = 0;
Lightvalve 13:747daba9cf59 189 int Ref_Valve_Pos_FF = DDV_CENTER;
Lightvalve 13:747daba9cf59 190 for(i=0; i<VALVE_POS_NUM; i++)
Lightvalve 13:747daba9cf59 191 {
Lightvalve 13:747daba9cf59 192 if(REF_JOINT_VEL >= min(JOINT_VEL[i],JOINT_VEL[i+1]) && REF_JOINT_VEL <= max(JOINT_VEL[i],JOINT_VEL[i+1]))
Lightvalve 13:747daba9cf59 193 {
Lightvalve 13:747daba9cf59 194 if(i==0)
Lightvalve 13:747daba9cf59 195 {
Lightvalve 13:747daba9cf59 196 Ref_Valve_Pos_FF = ((int) 50.0/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER;
Lightvalve 13:747daba9cf59 197 }
Lightvalve 13:747daba9cf59 198 else
Lightvalve 13:747daba9cf59 199 {
Lightvalve 13:747daba9cf59 200 Ref_Valve_Pos_FF = ((int) 50.0*(ID_index_array[i+1] - ID_index_array[i-1])/(JOINT_VEL[i+1] - JOINT_VEL[i-1]) * (REF_JOINT_VEL - JOINT_VEL[i-1])) + DDV_CENTER + 50*ID_index_array[i-1];
Lightvalve 13:747daba9cf59 201 }
Lightvalve 13:747daba9cf59 202 break;
Lightvalve 13:747daba9cf59 203 }
Lightvalve 13:747daba9cf59 204 }
Lightvalve 13:747daba9cf59 205 if(REF_JOINT_VEL > max(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2]))
Lightvalve 13:747daba9cf59 206 {
Lightvalve 13:747daba9cf59 207 Ref_Valve_Pos_FF = VALVE_MAX_POS;
Lightvalve 13:747daba9cf59 208 }
Lightvalve 13:747daba9cf59 209 else if(REF_JOINT_VEL < min(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2]))
Lightvalve 13:747daba9cf59 210 {
Lightvalve 13:747daba9cf59 211 Ref_Valve_Pos_FF = VALVE_MIN_POS;
Lightvalve 13:747daba9cf59 212 }
Lightvalve 13:747daba9cf59 213 return Ref_Valve_Pos_FF;
Lightvalve 13:747daba9cf59 214 }
jobuuu 6:df07d3491e3a 215
jobuuu 6:df07d3491e3a 216
Lightvalve 13:747daba9cf59 217 void VALVE_POS_CONTROL(double REF_VALVE_POS){
Lightvalve 13:747daba9cf59 218 int i = 0;
Lightvalve 13:747daba9cf59 219
Lightvalve 13:747daba9cf59 220 valve_pos_err = REF_VALVE_POS - value;
Lightvalve 13:747daba9cf59 221 valve_pos_err_diff = valve_pos_err - valve_pos_err_old;
Lightvalve 13:747daba9cf59 222 valve_pos_err_old = valve_pos_err;
Lightvalve 13:747daba9cf59 223 valve_pos_err_sum += valve_pos_err;
Lightvalve 13:747daba9cf59 224 if (valve_pos_err_sum > 1000) valve_pos_err_sum = 1000;
Lightvalve 13:747daba9cf59 225 if (valve_pos_err_sum<-1000) valve_pos_err_sum = -1000;
Lightvalve 13:747daba9cf59 226
Lightvalve 13:747daba9cf59 227 VALVE_PWM_RAW_FB = P_GAIN_VALVE_POSITION * valve_pos_err + I_GAIN_VALVE_POSITION * valve_pos_err_sum + D_GAIN_VALVE_POSITION * valve_pos_err_diff;
Lightvalve 13:747daba9cf59 228
Lightvalve 13:747daba9cf59 229 if(REF_VALVE_POS > VALVE_MAX_POS)
Lightvalve 13:747daba9cf59 230 {
Lightvalve 13:747daba9cf59 231 REF_VALVE_POS = VALVE_MAX_POS;
Lightvalve 13:747daba9cf59 232 }
Lightvalve 13:747daba9cf59 233 else if(REF_VALVE_POS < VALVE_MIN_POS)
Lightvalve 13:747daba9cf59 234 {
Lightvalve 13:747daba9cf59 235 REF_VALVE_POS = VALVE_MIN_POS;
Lightvalve 13:747daba9cf59 236 }
Lightvalve 13:747daba9cf59 237
Lightvalve 13:747daba9cf59 238 for(i=0; i<16; i++)
Lightvalve 13:747daba9cf59 239 {
Lightvalve 13:747daba9cf59 240 if(REF_VALVE_POS >= min(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1]) && REF_VALVE_POS <= max(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1]))
Lightvalve 13:747daba9cf59 241 {
Lightvalve 13:747daba9cf59 242 if(i==0)
Lightvalve 13:747daba9cf59 243 {
Lightvalve 13:747daba9cf59 244 VALVE_PWM_RAW_FF = (double) 1000.0/(VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i]) * (REF_VALVE_POS - VALVE_POS_VS_PWM[i]);
Lightvalve 13:747daba9cf59 245 }
Lightvalve 13:747daba9cf59 246 else
Lightvalve 13:747daba9cf59 247 {
Lightvalve 13:747daba9cf59 248 VALVE_PWM_RAW_FF = (double) 1000.0*(ID_index_array[i+1] - ID_index_array[i-1])/(VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i-1]) * (REF_VALVE_POS - VALVE_POS_VS_PWM[i-1]) + 1000*ID_index_array[i-1];
Lightvalve 13:747daba9cf59 249 }
Lightvalve 13:747daba9cf59 250 break;
Lightvalve 13:747daba9cf59 251 }
Lightvalve 13:747daba9cf59 252 }
Lightvalve 13:747daba9cf59 253 VALVE_PWM_RAW = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
Lightvalve 13:747daba9cf59 254 }
Lightvalve 13:747daba9cf59 255
Lightvalve 13:747daba9cf59 256
Lightvalve 13:747daba9cf59 257 double PWM_duty_byLT(double Ref_V) {
Lightvalve 13:747daba9cf59 258 double PWM_duty = 0.0;
Lightvalve 13:747daba9cf59 259 if(Ref_V<LT_Voltage_Output[0]) {
Lightvalve 13:747daba9cf59 260 PWM_duty = (Ref_V-LT_Voltage_Output[0])/1.5+LT_PWM_duty[0];
Lightvalve 13:747daba9cf59 261 } else if (Ref_V>=LT_Voltage_Output[LT_MAX_IDX-1]) {
Lightvalve 13:747daba9cf59 262 PWM_duty = (Ref_V-LT_Voltage_Output[LT_MAX_IDX-1])/1.5+LT_PWM_duty[LT_MAX_IDX-1];
Lightvalve 13:747daba9cf59 263 } else {
Lightvalve 13:747daba9cf59 264 int idx = 0;
Lightvalve 13:747daba9cf59 265 for(idx=0; idx<LT_MAX_IDX-1; idx++) {
Lightvalve 13:747daba9cf59 266 double ini_x = LT_Voltage_Output[idx];
Lightvalve 13:747daba9cf59 267 double fin_x = LT_Voltage_Output[idx+1];
Lightvalve 13:747daba9cf59 268 double ini_y = LT_PWM_duty[idx];
Lightvalve 13:747daba9cf59 269 double fin_y = LT_PWM_duty[idx+1];
Lightvalve 13:747daba9cf59 270 if(Ref_V>=ini_x && Ref_V<fin_x) {
Lightvalve 13:747daba9cf59 271 PWM_duty = (fin_y-ini_y)/(fin_x-ini_x)*(Ref_V-ini_x) + ini_y;
Lightvalve 13:747daba9cf59 272 break;
Lightvalve 13:747daba9cf59 273 }
Lightvalve 13:747daba9cf59 274 }
Lightvalve 13:747daba9cf59 275 }
Lightvalve 13:747daba9cf59 276
Lightvalve 13:747daba9cf59 277 return PWM_duty;
Lightvalve 13:747daba9cf59 278 }
jobuuu 6:df07d3491e3a 279
jobuuu 2:a1c0a37df760 280 /*******************************************************************************
jobuuu 2:a1c0a37df760 281 TIMER INTERRUPT
jobuuu 2:a1c0a37df760 282 *******************************************************************************/
jobuuu 2:a1c0a37df760 283
jobuuu 2:a1c0a37df760 284 unsigned long CNT_TMR4 = 0;
jobuuu 2:a1c0a37df760 285 double FREQ_TMR4 = (double)FREQ_20k;
jobuuu 5:a4319f79457b 286 double DT_TMR4 = (double)DT_20k;
jobuuu 1:e04e563be5ce 287 extern "C" void TIM4_IRQHandler(void)
jobuuu 1:e04e563be5ce 288 {
jobuuu 2:a1c0a37df760 289 if ( TIM4->SR & TIM_SR_UIF ) {
jobuuu 1:e04e563be5ce 290 /*******************************************************
Lightvalve 13:747daba9cf59 291 *** Sensor Read & Data Handling
jobuuu 1:e04e563be5ce 292 ********************************************************/
Lightvalve 13:747daba9cf59 293
Lightvalve 13:747daba9cf59 294 if((CNT_TMR4%2)==0) {
Lightvalve 13:747daba9cf59 295
jobuuu 2:a1c0a37df760 296 //ADC
jobuuu 5:a4319f79457b 297 ADC3->CR2 |= 0x40000000; // adc _ 12bit
Lightvalve 11:82d8768d7351 298 // a1=ADC1->DR;
Lightvalve 11:82d8768d7351 299 // a1=ADC2->DR;
Lightvalve 11:82d8768d7351 300 // int raw_cur = ADC3->DR;
jobuuu 5:a4319f79457b 301 while((ADC3->SR & 0b10));
Lightvalve 13:747daba9cf59 302
jobuuu 5:a4319f79457b 303 double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz
jobuuu 5:a4319f79457b 304 double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA
jobuuu 7:e9086c72bb22 305 cur.sen=cur.sen*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur);
jobuuu 2:a1c0a37df760 306 }
jobuuu 1:e04e563be5ce 307
jobuuu 1:e04e563be5ce 308 //DAC
jobuuu 1:e04e563be5ce 309 // dac_1 = ADC1->DR;
jobuuu 1:e04e563be5ce 310 // dac_2 = ADC2->DR;
Lightvalve 11:82d8768d7351 311
Lightvalve 13:747daba9cf59 312
Lightvalve 13:747daba9cf59 313
Lightvalve 11:82d8768d7351 314 /*******************************************************
Lightvalve 11:82d8768d7351 315 *** Timer Counting & etc.
Lightvalve 11:82d8768d7351 316 ********************************************************/
Lightvalve 13:747daba9cf59 317 CNT_TMR4++;
Lightvalve 11:82d8768d7351 318 }
Lightvalve 11:82d8768d7351 319 TIM4->SR = 0x0; // reset the status register
Lightvalve 11:82d8768d7351 320 }
Lightvalve 11:82d8768d7351 321
Lightvalve 11:82d8768d7351 322 unsigned long CNT_TMR3 = 0;
Lightvalve 11:82d8768d7351 323 double FREQ_TMR3 = (double)FREQ_5k;
Lightvalve 11:82d8768d7351 324 double DT_TMR3 = (double)DT_5k;
Lightvalve 11:82d8768d7351 325 extern "C" void TIM3_IRQHandler(void)
Lightvalve 11:82d8768d7351 326 {
Lightvalve 11:82d8768d7351 327 if ( TIM3->SR & TIM_SR_UIF ) {
Lightvalve 13:747daba9cf59 328
Lightvalve 12:6f2531038ea4 329 ENC_UPDATE();
Lightvalve 12:6f2531038ea4 330 CUR_PRES_A_BAR = (CUR_PRES_A - PRES_A_NULL) / PRES_SENSOR_A_PULSE_PER_BAR + 1.;
Lightvalve 12:6f2531038ea4 331 CUR_PRES_B_BAR = (CUR_PRES_B - PRES_B_NULL) / PRES_SENSOR_B_PULSE_PER_BAR + 1.;
Lightvalve 12:6f2531038ea4 332 //CUR_TORQUE_NM = (CUR_TORQUE - TORQUE_NULL) / (double) TORQUE_SENSOR_PULSE_PER_TORQUE; //
Lightvalve 12:6f2531038ea4 333 CUR_TORQUE_NM = CUR_TORQUE; //
Lightvalve 12:6f2531038ea4 334 CUR_TORQUE_NM_PRESS = 1. * (CUR_PRES_A_BAR - CUR_PRES_B_BAR);
Lightvalve 13:747daba9cf59 335
Lightvalve 13:747daba9cf59 336 // Reference Loop
Lightvalve 12:6f2531038ea4 337 switch (REFERENCE_MODE) {
Lightvalve 13:747daba9cf59 338 case MODE_REF_NO_ACT: {
Lightvalve 12:6f2531038ea4 339 break;
Lightvalve 12:6f2531038ea4 340 }
Lightvalve 13:747daba9cf59 341
Lightvalve 13:747daba9cf59 342 case MODE_REF_DIRECT: {
Lightvalve 12:6f2531038ea4 343 if (FLAG_REFERENCE_VALVE_PWM) {
Lightvalve 12:6f2531038ea4 344 Ref_PWM = (double) REF_PWM;
Lightvalve 12:6f2531038ea4 345 }
Lightvalve 12:6f2531038ea4 346 if (FLAG_REFERENCE_VALVE_POSITION) {
Lightvalve 12:6f2531038ea4 347 Ref_Valve_Pos = (double) REF_VALVE_POSITION;
Lightvalve 12:6f2531038ea4 348 }
Lightvalve 12:6f2531038ea4 349 if (FLAG_REFERENCE_JOINT_POSITION) {
Lightvalve 12:6f2531038ea4 350 Ref_Joint_Pos = (double) REF_POSITION;
Lightvalve 12:6f2531038ea4 351 Ref_Joint_Vel = (double) REF_VELOCITY;
Lightvalve 12:6f2531038ea4 352 }
Lightvalve 12:6f2531038ea4 353 if (FLAG_REFERENCE_JOINT_TORQUE) {
Lightvalve 12:6f2531038ea4 354 Ref_Joint_Torq = (double) REF_TORQUE;
Lightvalve 12:6f2531038ea4 355 }
Lightvalve 12:6f2531038ea4 356 break;
Lightvalve 12:6f2531038ea4 357 }
Lightvalve 13:747daba9cf59 358
Lightvalve 13:747daba9cf59 359 case MODE_REF_COS_INC: {
Lightvalve 12:6f2531038ea4 360 if (FLAG_REFERENCE_VALVE_PWM) {
Lightvalve 12:6f2531038ea4 361 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 362 }
Lightvalve 12:6f2531038ea4 363 if (FLAG_REFERENCE_VALVE_POSITION) {
Lightvalve 12:6f2531038ea4 364 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 365 }
Lightvalve 12:6f2531038ea4 366 if (FLAG_REFERENCE_JOINT_POSITION) {
Lightvalve 12:6f2531038ea4 367 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 368 Ref_Joint_Vel = 0.0;
Lightvalve 12:6f2531038ea4 369 }
Lightvalve 12:6f2531038ea4 370 if (FLAG_REFERENCE_JOINT_TORQUE) {
Lightvalve 12:6f2531038ea4 371 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 372 }
Lightvalve 12:6f2531038ea4 373 TMR2_COUNT_REFERENCE++;
Lightvalve 12:6f2531038ea4 374 if (TMR2_COUNT_REFERENCE >= REF_MOVE_TIME_5k) {
Lightvalve 12:6f2531038ea4 375 REFERENCE_MODE = MODE_REF_DIRECT;
Lightvalve 12:6f2531038ea4 376 TMR2_COUNT_REFERENCE = 0;
Lightvalve 12:6f2531038ea4 377 }
Lightvalve 12:6f2531038ea4 378 break;
Lightvalve 12:6f2531038ea4 379 }
Lightvalve 13:747daba9cf59 380
Lightvalve 13:747daba9cf59 381
Lightvalve 13:747daba9cf59 382 case MODE_REF_LINE_INC: {
Lightvalve 12:6f2531038ea4 383 if (FLAG_REFERENCE_VALVE_PWM) {
Lightvalve 12:6f2531038ea4 384 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 385 }
Lightvalve 12:6f2531038ea4 386 if (FLAG_REFERENCE_VALVE_POSITION) {
Lightvalve 12:6f2531038ea4 387 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 388 }
Lightvalve 12:6f2531038ea4 389 if (FLAG_REFERENCE_JOINT_POSITION) {
Lightvalve 12:6f2531038ea4 390 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 391 Ref_Vel_Test = ((double) REF_POSITION - (double) INIT_REF_POS) / (double) REF_MOVE_TIME_5k * (double) TMR_FREQ_5k; // pulse/sec
Lightvalve 12:6f2531038ea4 392 //Ref_Vel_Test = 10;
Lightvalve 12:6f2531038ea4 393 }
Lightvalve 12:6f2531038ea4 394 if (FLAG_REFERENCE_JOINT_TORQUE) {
Lightvalve 12:6f2531038ea4 395 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 396 }
Lightvalve 12:6f2531038ea4 397 TMR2_COUNT_REFERENCE++;
Lightvalve 12:6f2531038ea4 398 if (TMR2_COUNT_REFERENCE >= REF_MOVE_TIME_5k) {
Lightvalve 12:6f2531038ea4 399 //REFERENCE_MODE = MODE_REF_DIRECT;
Lightvalve 12:6f2531038ea4 400 Ref_Vel_Test = 1;
Lightvalve 12:6f2531038ea4 401 TMR2_COUNT_REFERENCE = REF_MOVE_TIME_5k;
Lightvalve 12:6f2531038ea4 402 //TMR2_COUNT_REFERENCE = 0;
Lightvalve 12:6f2531038ea4 403 }
Lightvalve 12:6f2531038ea4 404 break;
Lightvalve 12:6f2531038ea4 405 }
Lightvalve 13:747daba9cf59 406
Lightvalve 13:747daba9cf59 407 case MODE_REF_SIN_WAVE: {
Lightvalve 12:6f2531038ea4 408 if (FLAG_REFERENCE_VALVE_PWM) {
Lightvalve 12:6f2531038ea4 409 Ref_PWM = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_PWM;
Lightvalve 12:6f2531038ea4 410 }
Lightvalve 12:6f2531038ea4 411 if (FLAG_REFERENCE_VALVE_POSITION) {
Lightvalve 12:6f2531038ea4 412 Ref_Valve_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_VALVE_POSITION;
Lightvalve 12:6f2531038ea4 413 }
Lightvalve 12:6f2531038ea4 414 if (FLAG_REFERENCE_JOINT_POSITION) {
Lightvalve 12:6f2531038ea4 415 Ref_Joint_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_POSITION;
Lightvalve 12:6f2531038ea4 416 }
Lightvalve 12:6f2531038ea4 417 if (FLAG_REFERENCE_JOINT_TORQUE) {
Lightvalve 12:6f2531038ea4 418 Ref_Joint_Torq = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_TORQUE;
Lightvalve 12:6f2531038ea4 419 }
Lightvalve 12:6f2531038ea4 420 TMR2_COUNT_REFERENCE++;
Lightvalve 12:6f2531038ea4 421 if (TMR2_COUNT_REFERENCE >= TMR2_COUNT_REFERENCE * REF_NUM) {
Lightvalve 12:6f2531038ea4 422 REFERENCE_MODE = MODE_REF_DIRECT;
Lightvalve 12:6f2531038ea4 423 TMR2_COUNT_REFERENCE = 0;
Lightvalve 12:6f2531038ea4 424 }
Lightvalve 12:6f2531038ea4 425 break;
Lightvalve 12:6f2531038ea4 426 }
Lightvalve 13:747daba9cf59 427
Lightvalve 13:747daba9cf59 428 case MODE_REF_SQUARE_WAVE: {
Lightvalve 12:6f2531038ea4 429 if (FLAG_REFERENCE_VALVE_PWM) {
Lightvalve 12:6f2531038ea4 430 Ref_PWM = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_PWM;
Lightvalve 12:6f2531038ea4 431 if (Ref_PWM >= REF_PWM) Ref_PWM = REF_MAG + REF_PWM;
Lightvalve 12:6f2531038ea4 432 else Ref_PWM = -REF_MAG + REF_PWM;
Lightvalve 12:6f2531038ea4 433 }
Lightvalve 12:6f2531038ea4 434 if (FLAG_REFERENCE_VALVE_POSITION) {
Lightvalve 12:6f2531038ea4 435 Ref_Valve_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_VALVE_POSITION;
Lightvalve 12:6f2531038ea4 436 if (Ref_Valve_Pos >= REF_VALVE_POSITION) Ref_Valve_Pos = REF_MAG + REF_VALVE_POSITION;
Lightvalve 12:6f2531038ea4 437 else Ref_Valve_Pos = -REF_MAG + REF_VALVE_POSITION;
Lightvalve 12:6f2531038ea4 438 }
Lightvalve 12:6f2531038ea4 439 if (FLAG_REFERENCE_JOINT_POSITION) {
Lightvalve 12:6f2531038ea4 440 Ref_Joint_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_POSITION;
Lightvalve 12:6f2531038ea4 441 if (Ref_Joint_Pos >= REF_POSITION) Ref_Valve_Pos = REF_MAG + REF_POSITION;
Lightvalve 12:6f2531038ea4 442 else Ref_Joint_Pos = -REF_MAG + REF_POSITION;
Lightvalve 12:6f2531038ea4 443 }
Lightvalve 12:6f2531038ea4 444 if (FLAG_REFERENCE_JOINT_TORQUE) {
Lightvalve 12:6f2531038ea4 445 Ref_Joint_Torq = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_TORQUE;
Lightvalve 12:6f2531038ea4 446 if (Ref_Joint_Torq >= REF_TORQUE) Ref_Valve_Pos = REF_MAG + REF_TORQUE;
Lightvalve 12:6f2531038ea4 447 else Ref_Joint_Torq = -REF_MAG + REF_TORQUE;
Lightvalve 12:6f2531038ea4 448 }
Lightvalve 12:6f2531038ea4 449 TMR2_COUNT_REFERENCE++;
Lightvalve 12:6f2531038ea4 450 if (TMR2_COUNT_REFERENCE >= TMR2_COUNT_REFERENCE * REF_NUM) {
Lightvalve 12:6f2531038ea4 451 REFERENCE_MODE = MODE_REF_DIRECT;
Lightvalve 12:6f2531038ea4 452 TMR2_COUNT_REFERENCE = 0;
Lightvalve 12:6f2531038ea4 453 }
Lightvalve 12:6f2531038ea4 454 break;
Lightvalve 12:6f2531038ea4 455 }
Lightvalve 13:747daba9cf59 456
Lightvalve 13:747daba9cf59 457 default:
Lightvalve 13:747daba9cf59 458 break;
Lightvalve 13:747daba9cf59 459 }
Lightvalve 13:747daba9cf59 460
Lightvalve 13:747daba9cf59 461 // CONTROL LOOP ------------------------------------------------------------
Lightvalve 13:747daba9cf59 462 switch (CONTROL_MODE) {
Lightvalve 13:747daba9cf59 463 case MODE_NO_ACT:
Lightvalve 13:747daba9cf59 464 {
Lightvalve 13:747daba9cf59 465 // SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0);
Lightvalve 13:747daba9cf59 466 VALVE_PWM_RAW = 0;
Lightvalve 13:747daba9cf59 467 break;
Lightvalve 13:747daba9cf59 468 }
Lightvalve 12:6f2531038ea4 469
Lightvalve 13:747daba9cf59 470 case MODE_VALVE_OPEN_LOOP:
Lightvalve 13:747daba9cf59 471 {
Lightvalve 13:747daba9cf59 472 VALVE_PWM_RAW = Ref_PWM;
Lightvalve 13:747daba9cf59 473 break;
Lightvalve 13:747daba9cf59 474 }
Lightvalve 13:747daba9cf59 475
Lightvalve 13:747daba9cf59 476 case MODE_VALVE_POSITION_CONTROL:
Lightvalve 13:747daba9cf59 477 {
Lightvalve 13:747daba9cf59 478 VALVE_POS_CONTROL(Ref_Valve_Pos);
Lightvalve 13:747daba9cf59 479 break;
Lightvalve 13:747daba9cf59 480 }
Lightvalve 13:747daba9cf59 481
Lightvalve 13:747daba9cf59 482 case MODE_JOINT_POSITION_TORQUE_CONTROL_PWM:
Lightvalve 13:747daba9cf59 483 {
Lightvalve 13:747daba9cf59 484 double PWM_RAW_POS_FB = 0.0; // PWM by Position Feedback
Lightvalve 13:747daba9cf59 485 double PWM_RAW_POS_FF = 0.0; // PWM by Position Feedforward
Lightvalve 13:747daba9cf59 486 double PWM_RAW_FORCE_FB = 0.0; // PWM by Force Feedback
Lightvalve 13:747daba9cf59 487
Lightvalve 13:747daba9cf59 488 // feedback input for position control
Lightvalve 13:747daba9cf59 489 joint_pos_err = Ref_Joint_Pos - (double) CUR_POSITION;
Lightvalve 13:747daba9cf59 490 joint_pos_err_diff = joint_pos_err - joint_pos_err_old;
Lightvalve 13:747daba9cf59 491 joint_pos_err_old = joint_pos_err;
Lightvalve 13:747daba9cf59 492 joint_pos_err_sum += joint_pos_err;
Lightvalve 13:747daba9cf59 493 if (joint_pos_err_sum > 1000) joint_pos_err_sum = 1000;
Lightvalve 13:747daba9cf59 494 if (joint_pos_err_sum<-1000) joint_pos_err_sum = -1000;
Lightvalve 13:747daba9cf59 495 // PWM_RAW_POS_FB = (double) P_GAIN_JOINT_POSITION * joint_pos_err + (double) I_GAIN_JOINT_POSITION * joint_pos_err_sum + (double) D_GAIN_JOINT_POSITION * joint_pos_err_diff PWM_RAW_POS_FB = (double) P_GAIN_JOINT_POSITION * joint_pos_err + (double) I_GAIN_JOINT_POSITION * joint_pos_err_sum + (double) D_GAIN_JOINT_POSITION * joint_pos_err_diff;
Lightvalve 13:747daba9cf59 496 PWM_RAW_POS_FB = (double) P_GAIN_JOINT_POSITION * joint_pos_err;
Lightvalve 13:747daba9cf59 497 PWM_RAW_POS_FB = PWM_RAW_POS_FB * 0.01;
Lightvalve 13:747daba9cf59 498
Lightvalve 13:747daba9cf59 499 // feedforward input for position control
Lightvalve 13:747daba9cf59 500 double Ref_Joint_Vel_Act = Ref_Joint_Vel/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
Lightvalve 13:747daba9cf59 501 double K_ff = 0.9;
Lightvalve 13:747daba9cf59 502 if(Ref_Joint_Vel_Act > 0) K_ff = 0.90; // open
Lightvalve 13:747daba9cf59 503 if(Ref_Joint_Vel_Act > 0) K_ff = 0.75; // close
Lightvalve 13:747daba9cf59 504 PWM_RAW_POS_FF = K_ff*Ref_Joint_Vel_Act/0.50;
Lightvalve 13:747daba9cf59 505
Lightvalve 13:747daba9cf59 506 // torque feedback
Lightvalve 13:747daba9cf59 507 // joint_torq_err = Ref_Joint_Torq - CUR_TORQUE_NM;
Lightvalve 13:747daba9cf59 508 // joint_torq_err_diff = joint_torq_err - joint_torq_err_old;
Lightvalve 13:747daba9cf59 509 // joint_torq_err_old = joint_torq_err;
Lightvalve 13:747daba9cf59 510 // joint_torq_err_sum += joint_torq_err;
Lightvalve 13:747daba9cf59 511 // if (joint_torq_err_sum > 1000) joint_torq_err_sum = 1000;
Lightvalve 13:747daba9cf59 512 // if (joint_torq_err_sum<-1000) joint_torq_err_sum = -1000;
Lightvalve 13:747daba9cf59 513 // VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * joint_torq_err + (double) I_GAIN_JOINT_TORQUE * joint_torq_err_sum + (double) D_GAIN_JOINT_TORQUE * joint_torq_err_diff;
Lightvalve 13:747daba9cf59 514 // VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01;
Lightvalve 13:747daba9cf59 515 PWM_RAW_FORCE_FB = 0.0;
Lightvalve 13:747daba9cf59 516
Lightvalve 13:747daba9cf59 517 // VALVE_PWM_RAW = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_POS + (double) COMPLIANCE_GAIN * 0.01 * VALVE_PWM_RAW_TORQ;
Lightvalve 13:747daba9cf59 518 VALVE_PWM_RAW = PWM_RAW_POS_FF + PWM_RAW_POS_FB + PWM_RAW_FORCE_FB;
Lightvalve 13:747daba9cf59 519
Lightvalve 13:747daba9cf59 520 break;
Lightvalve 13:747daba9cf59 521 }
Lightvalve 13:747daba9cf59 522
Lightvalve 13:747daba9cf59 523 case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION:
Lightvalve 13:747daba9cf59 524 {
Lightvalve 13:747daba9cf59 525 TMR2_COUNT_JOINT++;
Lightvalve 13:747daba9cf59 526 double VALVE_POS_RAW_POS_FB = 0.0; // Valve Position by Position Feedback
Lightvalve 13:747daba9cf59 527 double VALVE_POS_RAW_POS_FF = 0.0; // Valve Position by Position Feedforward
Lightvalve 13:747daba9cf59 528 double VALVE_POS_RAW_FORCE_FB = 0.0; // Valve Position by Force Feedback
Lightvalve 13:747daba9cf59 529 int DDV_JOINT_CAN = 0;
Lightvalve 13:747daba9cf59 530 // feedback input for position control
Lightvalve 13:747daba9cf59 531 joint_pos_err = Ref_Joint_Pos - (double) CUR_POSITION;
Lightvalve 13:747daba9cf59 532 joint_pos_err_diff = joint_pos_err - joint_pos_err_old;
Lightvalve 13:747daba9cf59 533 joint_pos_err_old = joint_pos_err;
Lightvalve 13:747daba9cf59 534 joint_pos_err_sum += joint_pos_err;
Lightvalve 13:747daba9cf59 535 if (joint_pos_err_sum > 1000) joint_pos_err_sum = 1000;
Lightvalve 13:747daba9cf59 536 if (joint_pos_err_sum<-1000) joint_pos_err_sum = -1000;
Lightvalve 13:747daba9cf59 537 VALVE_POS_RAW_POS_FB = (double) P_GAIN_JOINT_POSITION * 0.01 * joint_pos_err + (double) I_GAIN_JOINT_POSITION * joint_pos_err_sum + (double) D_GAIN_JOINT_POSITION * joint_pos_err_diff;
Lightvalve 13:747daba9cf59 538 VALVE_POS_RAW_POS_FB = VALVE_POS_RAW_POS_FB * 0.01;
Lightvalve 13:747daba9cf59 539
Lightvalve 13:747daba9cf59 540
Lightvalve 13:747daba9cf59 541 //Ref_Joint_Vel = Ref_Vel_Test;
Lightvalve 13:747daba9cf59 542 // feedforward input for position control
Lightvalve 13:747daba9cf59 543 // double Ref_Joint_Vel_Act = Ref_Joint_Vel/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
Lightvalve 13:747daba9cf59 544 // double K_ff = 0.9;
Lightvalve 13:747daba9cf59 545 // if(Ref_Joint_Vel_Act > 0) K_ff = 0.90; // open
Lightvalve 13:747daba9cf59 546 // if(Ref_Joint_Vel_Act > 0) K_ff = 0.75; // close
Lightvalve 13:747daba9cf59 547 // VALVE_POS_RAW_POS_FF = K_ff*Ref_Joint_Vel_Act/0.50;
Lightvalve 13:747daba9cf59 548
Lightvalve 13:747daba9cf59 549 // torque feedback
Lightvalve 13:747daba9cf59 550 // joint_torq_err = Ref_Joint_Torq - CUR_TORQUE_NM;
Lightvalve 13:747daba9cf59 551 // joint_torq_err_diff = joint_torq_err - joint_torq_err_old;
Lightvalve 13:747daba9cf59 552 // joint_torq_err_old = joint_torq_err;
Lightvalve 13:747daba9cf59 553 // joint_torq_err_sum += joint_torq_err;
Lightvalve 13:747daba9cf59 554 // if (joint_torq_err_sum > 1000) joint_torq_err_sum = 1000;
Lightvalve 13:747daba9cf59 555 // if (joint_torq_err_sum<-1000) joint_torq_err_sum = -1000;
Lightvalve 13:747daba9cf59 556 // VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * joint_torq_err + (double) I_GAIN_JOINT_TORQUE * joint_torq_err_sum + (double) D_GAIN_JOINT_TORQUE * joint_torq_err_diff;
Lightvalve 13:747daba9cf59 557 // VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01;
Lightvalve 13:747daba9cf59 558 VALVE_POS_RAW_FORCE_FB = 0.0;
Lightvalve 13:747daba9cf59 559
Lightvalve 13:747daba9cf59 560 Ref_Valve_Pos = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(Ref_Joint_Vel) + VALVE_POS_RAW_FORCE_FB;
Lightvalve 13:747daba9cf59 561 //Ref_Valve_Pos = DDV_JOINT_POS_FF(Ref_Joint_Vel);
Lightvalve 13:747daba9cf59 562
Lightvalve 13:747daba9cf59 563 VALVE_POS_CONTROL(Ref_Valve_Pos);
Lightvalve 13:747daba9cf59 564 break;
Lightvalve 13:747daba9cf59 565 }
Lightvalve 13:747daba9cf59 566
Lightvalve 13:747daba9cf59 567 case MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING:
Lightvalve 13:747daba9cf59 568 {
Lightvalve 13:747daba9cf59 569
Lightvalve 13:747daba9cf59 570 break;
Lightvalve 13:747daba9cf59 571 }
Lightvalve 13:747daba9cf59 572
Lightvalve 13:747daba9cf59 573 case MODE_JOINT_POSITION_PRES_CONTROL_PWM:
Lightvalve 13:747daba9cf59 574 {
Lightvalve 13:747daba9cf59 575 joint_pos_err = Ref_Joint_Pos - (double) CUR_POSITION;
Lightvalve 13:747daba9cf59 576 joint_pos_err_diff = joint_pos_err - joint_pos_err_old;
Lightvalve 13:747daba9cf59 577 joint_pos_err_old = joint_pos_err;
Lightvalve 13:747daba9cf59 578 joint_pos_err_sum += joint_pos_err;
Lightvalve 13:747daba9cf59 579 if (joint_pos_err_sum > 1000) joint_pos_err_sum = 1000;
Lightvalve 13:747daba9cf59 580 if (joint_pos_err_sum<-1000) joint_pos_err_sum = -1000;
Lightvalve 13:747daba9cf59 581 VALVE_PWM_RAW_POS = ((double) P_GAIN_JOINT_POSITION * joint_pos_err + (double) I_GAIN_JOINT_POSITION * joint_pos_err_sum + (double) D_GAIN_JOINT_POSITION * joint_pos_err_diff) * 0.01;
Lightvalve 13:747daba9cf59 582
Lightvalve 13:747daba9cf59 583 joint_torq_err = Ref_Joint_Torq - CUR_TORQUE_NM;
Lightvalve 13:747daba9cf59 584 joint_torq_err_diff = joint_torq_err - joint_torq_err_old;
Lightvalve 13:747daba9cf59 585 joint_torq_err_old = joint_torq_err;
Lightvalve 13:747daba9cf59 586 joint_torq_err_sum += joint_torq_err;
Lightvalve 13:747daba9cf59 587 if (joint_torq_err_sum > 1000) joint_torq_err_sum = 1000;
Lightvalve 13:747daba9cf59 588 if (joint_torq_err_sum<-1000) joint_torq_err_sum = -1000;
Lightvalve 13:747daba9cf59 589 VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * joint_torq_err + (double) I_GAIN_JOINT_TORQUE * joint_torq_err_sum + (double) D_GAIN_JOINT_TORQUE * joint_torq_err_diff;
Lightvalve 13:747daba9cf59 590
Lightvalve 13:747daba9cf59 591 VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01;
Lightvalve 13:747daba9cf59 592
Lightvalve 13:747daba9cf59 593 VALVE_PWM_RAW = VALVE_PWM_RAW_POS + (double) COMPLIANCE_GAIN * 0.01 * VALVE_PWM_RAW_TORQ;
Lightvalve 13:747daba9cf59 594
Lightvalve 13:747daba9cf59 595 CUR_FLOWRATE = (double) CUR_VELOCITY * 0.00009587;
Lightvalve 13:747daba9cf59 596 CUR_FLOWRATE = CUR_FLOWRATE * 0.5757; // 0.4791=2*pi/65536*5000(pulse/tic to rad/s) 0.5757=0.02525*0.02*0.0095*2*60*1000 (radius * area * 2 * 60(sec --> min) * 1000(m^3 --> L))
Lightvalve 13:747daba9cf59 597 if (DIR_VALVE > 0) {
Lightvalve 13:747daba9cf59 598 if (CUR_FLOWRATE >= 0 && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[0]*1.0)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - 0.0) / (VALVE_GAIN_LPM_PER_V[0]*1.0 - 0.0) + 0.0;
Lightvalve 13:747daba9cf59 599 else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[0]*1.0) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[2]*2.)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[0]*1.0) / (VALVE_GAIN_LPM_PER_V[2]*2.0 - VALVE_GAIN_LPM_PER_V[0]*1.) + 1.0;
Lightvalve 13:747daba9cf59 600 else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[2]*2.0) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[4]*3.)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[2]*2.0) / (VALVE_GAIN_LPM_PER_V[4]*3.0 - VALVE_GAIN_LPM_PER_V[2]*2.) + 2.0;
Lightvalve 13:747daba9cf59 601 else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[4]*3.0) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[6]*4.)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[4]*3.0) / (VALVE_GAIN_LPM_PER_V[6]*4.0 - VALVE_GAIN_LPM_PER_V[4]*3.) + 3.0;
Lightvalve 13:747daba9cf59 602 else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[6]*4.0) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[8]*5.)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[6]*4.0) / (VALVE_GAIN_LPM_PER_V[8]*5.0 - VALVE_GAIN_LPM_PER_V[6]*4.) + 4.0;
Lightvalve 13:747daba9cf59 603 else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[8]*5.0)) VALVE_FF_VOLTAGE = 5.0;
Lightvalve 13:747daba9cf59 604 else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[1]*(-1.0)) && CUR_FLOWRATE < 0.0) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[1]*(-1.0)) / (0.0 - VALVE_GAIN_LPM_PER_V[1]*(-1.)) - 1.0;
Lightvalve 13:747daba9cf59 605 else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[3]*(-2.0)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[1]*(-1.0))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[3]*(-2.0)) / ((VALVE_GAIN_LPM_PER_V[1]*(-1.0)) - VALVE_GAIN_LPM_PER_V[3]*(-2.)) - 2.0;
Lightvalve 13:747daba9cf59 606 else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[5]*(-3.0)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[3]*(-2.0))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[5]*(-3.0)) / ((VALVE_GAIN_LPM_PER_V[3]*(-2.0)) - VALVE_GAIN_LPM_PER_V[5]*(-3.)) - 3.0;
Lightvalve 13:747daba9cf59 607 else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[7]*(-4.0)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[5]*(-3.0))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[7]*(-4.0)) / ((VALVE_GAIN_LPM_PER_V[5]*(-3.0)) - VALVE_GAIN_LPM_PER_V[7]*(-4.)) - 4.0;
Lightvalve 13:747daba9cf59 608 else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[9]*(-5.0)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[7]*(-4.0))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[9]*(-5.0)) / ((VALVE_GAIN_LPM_PER_V[7]*(-4.0)) - VALVE_GAIN_LPM_PER_V[9]*(-5.)) - 5.0;
Lightvalve 13:747daba9cf59 609 else if (CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[9]*(-5.0))) VALVE_FF_VOLTAGE = -5;
Lightvalve 13:747daba9cf59 610 else VALVE_FF_VOLTAGE = 0;
Lightvalve 13:747daba9cf59 611 } else {
Lightvalve 13:747daba9cf59 612 if (CUR_FLOWRATE >= 0 && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[0]*1.0)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - 0.0) / (VALVE_GAIN_LPM_PER_V[0]*1.0 - 0.0) + 0.0;
Lightvalve 13:747daba9cf59 613 else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[1]*1.0) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[2]*2.0)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[0]*1.0) / (VALVE_GAIN_LPM_PER_V[2]*2.0 - VALVE_GAIN_LPM_PER_V[0]*1.) + 1.0;
Lightvalve 13:747daba9cf59 614 else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[3]*2.0) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[4]*3.0)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[2]*2.0) / (VALVE_GAIN_LPM_PER_V[4]*3.0 - VALVE_GAIN_LPM_PER_V[2]*2.) + 2.0;
Lightvalve 13:747daba9cf59 615 else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[5]*3.0) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[6]*4.0)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[4]*3.0) / (VALVE_GAIN_LPM_PER_V[6]*4.0 - VALVE_GAIN_LPM_PER_V[4]*3.) + 3.0;
Lightvalve 13:747daba9cf59 616 else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[7]*4.0) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[8]*5.0)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[6]*4.0) / (VALVE_GAIN_LPM_PER_V[8]*5.0 - VALVE_GAIN_LPM_PER_V[6]*4.) + 4.0;
Lightvalve 13:747daba9cf59 617 else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[9]*5.0)) VALVE_FF_VOLTAGE = 5.0;
Lightvalve 13:747daba9cf59 618 else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[0]*(-1.0)) && CUR_FLOWRATE < 0.0) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[1]*(-1.0)) / (0.0 - VALVE_GAIN_LPM_PER_V[1]*(-1.0)) - 1.0;
Lightvalve 13:747daba9cf59 619 else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[2]*(-2.0)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[1]*(-1.0))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[3]*(-2.0)) / ((VALVE_GAIN_LPM_PER_V[1]*(-1.0)) - VALVE_GAIN_LPM_PER_V[3]*(-2.0)) - 2.0;
Lightvalve 13:747daba9cf59 620 else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[4]*(-3.0)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[3]*(-2.0))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[5]*(-3.0)) / ((VALVE_GAIN_LPM_PER_V[3]*(-2.0)) - VALVE_GAIN_LPM_PER_V[5]*(-3.0)) - 3.0;
Lightvalve 13:747daba9cf59 621 else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[6]*(-4.0)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[5]*(-3.0))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[7]*(-4.0)) / ((VALVE_GAIN_LPM_PER_V[5]*(-3.0)) - VALVE_GAIN_LPM_PER_V[7]*(-4.0)) - 4.0;
Lightvalve 13:747daba9cf59 622 else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[8]*(-5.0)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[7]*(-4.0))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[9]*(-5.0)) / ((VALVE_GAIN_LPM_PER_V[7]*(-4.0)) - VALVE_GAIN_LPM_PER_V[9]*(-5.0)) - 5.0;
Lightvalve 13:747daba9cf59 623 else if (CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[8]*(-5.0))) VALVE_FF_VOLTAGE = -5;
Lightvalve 13:747daba9cf59 624 else VALVE_FF_VOLTAGE = 0;
Lightvalve 13:747daba9cf59 625 }
Lightvalve 13:747daba9cf59 626 // VALVE_FF_VOLTAGE = CUR_FLOWRATE * 0.5;
Lightvalve 13:747daba9cf59 627
Lightvalve 13:747daba9cf59 628 if (CUR_FLOWRATE >= 0) VALVE_FF_VOLTAGE = (double) VELOCITY_COMP_GAIN * 0.001 * VALVE_FF_VOLTAGE * sqrt((double) PRES_SUPPLY - CUR_PRES_A_BAR) * 0.0707; // 0.0707 = 1/sqrt(200.))
Lightvalve 13:747daba9cf59 629 else if (CUR_FLOWRATE < 0) VALVE_FF_VOLTAGE = (double) VELOCITY_COMP_GAIN * 0.001 * VALVE_FF_VOLTAGE * sqrt((double) PRES_SUPPLY - CUR_PRES_B_BAR) * 0.0707;
Lightvalve 13:747daba9cf59 630
Lightvalve 13:747daba9cf59 631 VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_FF_VOLTAGE * PWM_RESOL / SUPPLY_VOLTAGE;
Lightvalve 13:747daba9cf59 632 break;
Lightvalve 13:747daba9cf59 633 }
Lightvalve 13:747daba9cf59 634
Lightvalve 13:747daba9cf59 635 case MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION:
Lightvalve 13:747daba9cf59 636 {
Lightvalve 13:747daba9cf59 637 joint_pos_err = Ref_Joint_Pos - (double) CUR_POSITION;
Lightvalve 13:747daba9cf59 638 joint_pos_err_diff = joint_pos_err - joint_pos_err_old;
Lightvalve 13:747daba9cf59 639 joint_pos_err_old = joint_pos_err;
Lightvalve 13:747daba9cf59 640 joint_pos_err_sum += joint_pos_err;
Lightvalve 13:747daba9cf59 641 if (joint_pos_err_sum > 1000) joint_pos_err_sum = 1000;
Lightvalve 13:747daba9cf59 642 if (joint_pos_err_sum<-1000) joint_pos_err_sum = -1000;
Lightvalve 13:747daba9cf59 643 VALVE_PWM_RAW_POS = ((double) P_GAIN_JOINT_POSITION * joint_pos_err + (double) I_GAIN_JOINT_POSITION * joint_pos_err_sum + (double) D_GAIN_JOINT_POSITION * joint_pos_err_diff) * 0.01;
Lightvalve 13:747daba9cf59 644
Lightvalve 13:747daba9cf59 645 joint_torq_err = Ref_Joint_Torq - CUR_TORQUE_NM;
Lightvalve 13:747daba9cf59 646 joint_torq_err_diff = joint_torq_err - joint_torq_err_old;
Lightvalve 13:747daba9cf59 647 joint_torq_err_old = joint_torq_err;
Lightvalve 13:747daba9cf59 648 joint_torq_err_sum += joint_torq_err;
Lightvalve 13:747daba9cf59 649 if (joint_torq_err_sum > 1000) joint_torq_err_sum = 1000;
Lightvalve 13:747daba9cf59 650 if (joint_torq_err_sum<-1000) joint_torq_err_sum = -1000;
Lightvalve 13:747daba9cf59 651 VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * joint_torq_err + (double) I_GAIN_JOINT_TORQUE * joint_torq_err_sum + (double) D_GAIN_JOINT_TORQUE * joint_torq_err_diff;
Lightvalve 13:747daba9cf59 652
Lightvalve 13:747daba9cf59 653 Ref_Valve_Pos = VALVE_PWM_RAW_POS + VALVE_PWM_RAW_TORQ;
Lightvalve 13:747daba9cf59 654
Lightvalve 13:747daba9cf59 655 valve_pos_err = Ref_Valve_Pos - value;
Lightvalve 13:747daba9cf59 656 valve_pos_err_diff = valve_pos_err - valve_pos_err_old;
Lightvalve 13:747daba9cf59 657 valve_pos_err_old = valve_pos_err;
Lightvalve 13:747daba9cf59 658 valve_pos_err_sum += valve_pos_err;
Lightvalve 13:747daba9cf59 659 if (valve_pos_err_sum > 1000) valve_pos_err_sum = 1000;
Lightvalve 13:747daba9cf59 660 if (valve_pos_err_sum<-1000) valve_pos_err_sum = -1000;
Lightvalve 13:747daba9cf59 661
Lightvalve 13:747daba9cf59 662 VALVE_PWM_RAW = (double) P_GAIN_VALVE_POSITION * valve_pos_err + (double) I_GAIN_VALVE_POSITION * valve_pos_err_sum + (double) D_GAIN_VALVE_POSITION * valve_pos_err_diff;
Lightvalve 13:747daba9cf59 663
Lightvalve 13:747daba9cf59 664 break;
Lightvalve 13:747daba9cf59 665 }
Lightvalve 13:747daba9cf59 666
Lightvalve 13:747daba9cf59 667 case MODE_VALVE_POSITION_PRES_CONTROL_LEARNING:
Lightvalve 13:747daba9cf59 668 {
Lightvalve 13:747daba9cf59 669
Lightvalve 13:747daba9cf59 670 break;
Lightvalve 13:747daba9cf59 671 }
Lightvalve 13:747daba9cf59 672
Lightvalve 13:747daba9cf59 673 case MODE_TORQUE_SENSOR_NULLING:
Lightvalve 13:747daba9cf59 674 {
Lightvalve 13:747daba9cf59 675 // DAC Voltage reference set
Lightvalve 13:747daba9cf59 676 if (TMR2_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) {
Lightvalve 13:747daba9cf59 677 CUR_TORQUE_sum += CUR_TORQUE;
Lightvalve 13:747daba9cf59 678
Lightvalve 13:747daba9cf59 679 if (TMR2_COUNT_TORQUE_NULL % 10 == 0) {
Lightvalve 13:747daba9cf59 680 CUR_TORQUE_mean = CUR_TORQUE_sum / 10.0;
Lightvalve 13:747daba9cf59 681 CUR_TORQUE_sum = 0;
Lightvalve 13:747daba9cf59 682
Lightvalve 13:747daba9cf59 683 TORQUE_VREF += 0.0001 * (TORQUE_NULL - CUR_TORQUE_mean);
Lightvalve 13:747daba9cf59 684
Lightvalve 13:747daba9cf59 685 // TORQUE_VREF = 0;
Lightvalve 13:747daba9cf59 686 if (TORQUE_VREF > 3.3) TORQUE_VREF = 3.3;
Lightvalve 13:747daba9cf59 687 if (TORQUE_VREF < 0) TORQUE_VREF = 0;
Lightvalve 13:747daba9cf59 688
Lightvalve 13:747daba9cf59 689 spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0));
Lightvalve 13:747daba9cf59 690 spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0));
Lightvalve 13:747daba9cf59 691 }
Lightvalve 13:747daba9cf59 692 } else {
Lightvalve 13:747daba9cf59 693 CONTROL_MODE = MODE_NO_ACT;
Lightvalve 13:747daba9cf59 694 TMR2_COUNT_TORQUE_NULL = 0;
Lightvalve 13:747daba9cf59 695 CUR_TORQUE_sum = 0;
Lightvalve 13:747daba9cf59 696 CUR_TORQUE_mean = 0;
Lightvalve 13:747daba9cf59 697 spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0));
Lightvalve 13:747daba9cf59 698
Lightvalve 13:747daba9cf59 699 }
Lightvalve 13:747daba9cf59 700 TMR2_COUNT_TORQUE_NULL++;
Lightvalve 13:747daba9cf59 701 break;
Lightvalve 13:747daba9cf59 702 }
Lightvalve 13:747daba9cf59 703
Lightvalve 13:747daba9cf59 704 case MODE_VALVE_NULLING_AND_DEADZONE_SETTING:
Lightvalve 13:747daba9cf59 705 {
Lightvalve 13:747daba9cf59 706 if (TMR2_COUNT_DEADZONE == 0) {
Lightvalve 13:747daba9cf59 707 if (pos_plus_end == pos_minus_end) need_enc_init = true;
Lightvalve 13:747daba9cf59 708 else temp_time = 0;
Lightvalve 13:747daba9cf59 709 }
Lightvalve 13:747daba9cf59 710 if (need_enc_init) {
Lightvalve 13:747daba9cf59 711 if (TMR2_COUNT_DEADZONE < (int) (0.5 * (double) TMR_FREQ_5k)) {
Lightvalve 13:747daba9cf59 712 VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
Lightvalve 13:747daba9cf59 713 pos_plus_end = CUR_POSITION;
Lightvalve 13:747daba9cf59 714 } else if (TMR2_COUNT_DEADZONE < TMR_FREQ_5k) {
Lightvalve 13:747daba9cf59 715 VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
Lightvalve 13:747daba9cf59 716 pos_minus_end = CUR_POSITION;
Lightvalve 13:747daba9cf59 717 } else if (TMR2_COUNT_DEADZONE == TMR_FREQ_5k) need_enc_init = false;
Lightvalve 13:747daba9cf59 718 temp_time = TMR_FREQ_5k;
Lightvalve 13:747daba9cf59 719 }
Lightvalve 13:747daba9cf59 720
Lightvalve 13:747daba9cf59 721 if (temp_time <= TMR2_COUNT_DEADZONE && TMR2_COUNT_DEADZONE < (temp_time + TMR_FREQ_5k)) {
Lightvalve 13:747daba9cf59 722 VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION);
Lightvalve 13:747daba9cf59 723 VALVE_CENTER = VALVE_DEADZONE_PLUS = VALVE_DEADZONE_MINUS = 0;
Lightvalve 13:747daba9cf59 724
Lightvalve 13:747daba9cf59 725 } else if (temp_time <= TMR2_COUNT_DEADZONE && TMR2_COUNT_DEADZONE < (temp_time + (int) (1.9 * (double) TMR_FREQ_5k))) {
Lightvalve 13:747daba9cf59 726 VALVE_PWM_RAW = 0;
Lightvalve 13:747daba9cf59 727 CUR_VELOCITY_sum += CUR_VELOCITY;
Lightvalve 13:747daba9cf59 728 } else if (TMR2_COUNT_DEADZONE == (temp_time + 2 * TMR_FREQ_5k)) {
Lightvalve 13:747daba9cf59 729 if (CUR_VELOCITY_sum == 0) DZ_dir = 1;
Lightvalve 13:747daba9cf59 730 else if (CUR_VELOCITY_sum > 0) DZ_dir = 1;
Lightvalve 13:747daba9cf59 731 else if (CUR_VELOCITY_sum < 0) DZ_dir = -1;
Lightvalve 13:747daba9cf59 732 else DZ_temp_cnt2 = DZ_end;
Lightvalve 13:747daba9cf59 733 CUR_VELOCITY_sum = 0;
Lightvalve 13:747daba9cf59 734 } else if (TMR2_COUNT_DEADZONE > (temp_time + 2 * TMR_FREQ_5k)) {
Lightvalve 13:747daba9cf59 735 if (TMR2_COUNT_DEADZONE > (temp_time + 10 * TMR_FREQ_5k)) DZ_temp_cnt2 = DZ_end;
Lightvalve 13:747daba9cf59 736
Lightvalve 13:747daba9cf59 737 // Position of Dead Zone
Lightvalve 13:747daba9cf59 738 // (CUR_VELOCITY < 0) (CUR_VELOCITY == 0) (CUR_VELOCITY > 0)
Lightvalve 13:747daba9cf59 739 // | / | / |/
Lightvalve 13:747daba9cf59 740 // | ______/ ___|___/ ______/|
Lightvalve 13:747daba9cf59 741 // |/ / | / |
Lightvalve 13:747daba9cf59 742 // /| / | / |
Lightvalve 13:747daba9cf59 743 // 0V 0V 0V
Lightvalve 13:747daba9cf59 744
Lightvalve 13:747daba9cf59 745 if (DZ_temp_cnt2 < DZ_end) {
Lightvalve 13:747daba9cf59 746 if (TMR2_COUNT_DEADZONE % 20 != 0) {
Lightvalve 13:747daba9cf59 747 CUR_VELOCITY_sum += CUR_VELOCITY;
Lightvalve 13:747daba9cf59 748 } else {
Lightvalve 13:747daba9cf59 749 VALVE_PWM_RAW -= DZ_dir;
Lightvalve 13:747daba9cf59 750 if (CUR_VELOCITY_sum * DZ_dir < 0) DZ_temp_cnt++;
Lightvalve 13:747daba9cf59 751 CUR_VELOCITY_sum = 0;
Lightvalve 13:747daba9cf59 752 }
Lightvalve 13:747daba9cf59 753 if (DZ_temp_cnt == 5) {
Lightvalve 13:747daba9cf59 754 if (DZ_dir >= 0) VALVE_DEADZONE_MINUS = (int16_t) VALVE_PWM_RAW;
Lightvalve 13:747daba9cf59 755 else VALVE_DEADZONE_PLUS = (int16_t) VALVE_PWM_RAW;
Lightvalve 13:747daba9cf59 756 DZ_dir = -DZ_dir;
Lightvalve 13:747daba9cf59 757 DZ_temp_cnt = 0;
Lightvalve 13:747daba9cf59 758 DZ_temp_cnt2++;
Lightvalve 13:747daba9cf59 759 }
Lightvalve 13:747daba9cf59 760 } else {
Lightvalve 13:747daba9cf59 761 TMR2_COUNT_DEADZONE = -1;
Lightvalve 13:747daba9cf59 762 VALVE_CENTER = VALVE_DEADZONE_PLUS / 2 + VALVE_DEADZONE_MINUS / 2;
Lightvalve 13:747daba9cf59 763 if (VALVE_DEADZONE_PLUS < VALVE_DEADZONE_MINUS) {
Lightvalve 13:747daba9cf59 764 VALVE_DEADZONE_PLUS = VALVE_CENTER;
Lightvalve 13:747daba9cf59 765 VALVE_DEADZONE_MINUS = VALVE_CENTER;
Lightvalve 13:747daba9cf59 766 }
Lightvalve 13:747daba9cf59 767 VALVE_PWM_RAW = 0;
Lightvalve 13:747daba9cf59 768 spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS);
Lightvalve 13:747daba9cf59 769 spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS);
Lightvalve 13:747daba9cf59 770
Lightvalve 13:747daba9cf59 771 CONTROL_MODE = MODE_NO_ACT;
Lightvalve 13:747daba9cf59 772 DZ_temp_cnt2 = 0;
Lightvalve 13:747daba9cf59 773 }
Lightvalve 13:747daba9cf59 774 }
Lightvalve 13:747daba9cf59 775 TMR2_COUNT_DEADZONE++;
Lightvalve 13:747daba9cf59 776 break;
Lightvalve 13:747daba9cf59 777 }
Lightvalve 13:747daba9cf59 778
Lightvalve 13:747daba9cf59 779 case MODE_FIND_HOME:
Lightvalve 13:747daba9cf59 780 {
Lightvalve 13:747daba9cf59 781 if (FLAG_FIND_HOME == true) {
Lightvalve 13:747daba9cf59 782 cnt_findhome = 0;
Lightvalve 13:747daba9cf59 783 cnt_vel_findhome = 0;
Lightvalve 13:747daba9cf59 784 REFERENCE_MODE = MODE_REF_NO_ACT; // Stop taking reference data from PODO
Lightvalve 13:747daba9cf59 785 Ref_Joint_Pos = CUR_POSITION;
Lightvalve 13:747daba9cf59 786 FLAG_FIND_HOME = false;
Lightvalve 13:747daba9cf59 787 }
Lightvalve 13:747daba9cf59 788
Lightvalve 13:747daba9cf59 789 int check_enc = (TMR_FREQ_5k/10);
Lightvalve 13:747daba9cf59 790 if(cnt_findhome%check_enc == 0){
Lightvalve 13:747daba9cf59 791 FINDHOME_POSITION = CUR_POSITION;
Lightvalve 13:747daba9cf59 792 FINDHOME_VELOCITY = FINDHOME_POSITION - FINDHOME_POSITION_OLD;
Lightvalve 13:747daba9cf59 793 FINDHOME_POSITION_OLD = FINDHOME_POSITION;
Lightvalve 13:747daba9cf59 794 } cnt_findhome++;
Lightvalve 13:747daba9cf59 795 if(cnt_findhome == 10000) cnt_findhome = 0;
Lightvalve 13:747daba9cf59 796
Lightvalve 13:747daba9cf59 797 if (abs(FINDHOME_VELOCITY) <= 1) {
Lightvalve 13:747daba9cf59 798 cnt_vel_findhome = cnt_vel_findhome + 1;
Lightvalve 13:747daba9cf59 799 } else {
Lightvalve 13:747daba9cf59 800 cnt_vel_findhome = 0;
Lightvalve 13:747daba9cf59 801 }
Lightvalve 13:747daba9cf59 802
Lightvalve 13:747daba9cf59 803 if (cnt_vel_findhome < 2*TMR_FREQ_5k) {
Lightvalve 13:747daba9cf59 804 REFERENCE_MODE = MODE_REF_NO_ACT;
Lightvalve 13:747daba9cf59 805 if (HOMEPOS_OFFSET > 0) Ref_Joint_Pos = Ref_Joint_Pos + 1.0;
Lightvalve 13:747daba9cf59 806 else Ref_Joint_Pos = Ref_Joint_Pos - 1.0;
Lightvalve 13:747daba9cf59 807 joint_pos_err = Ref_Joint_Pos - CUR_POSITION;
Lightvalve 13:747daba9cf59 808 VALVE_PWM_RAW = ((double) P_GAIN_JOINT_POSITION * joint_pos_err) * 0.01;
Lightvalve 13:747daba9cf59 809 // if (HOMEPOS_OFFSET > 0) VALVE_PWM_RAW = 100;
Lightvalve 13:747daba9cf59 810 // else VALVE_PWM_RAW = -100;
Lightvalve 13:747daba9cf59 811 } else {
Lightvalve 13:747daba9cf59 812 ENC_SET(HOMEPOS_OFFSET);
Lightvalve 13:747daba9cf59 813 FLAG_REFERENCE_JOINT_POSITION = 1;
Lightvalve 13:747daba9cf59 814 INIT_REF_POS = HOMEPOS_OFFSET;
Lightvalve 13:747daba9cf59 815 REF_MOVE_TIME_5k = 2 * TMR_FREQ_5k;
Lightvalve 13:747daba9cf59 816 TMR2_COUNT_REFERENCE = 0;
Lightvalve 13:747daba9cf59 817 Ref_Joint_Vel = 0.0;
Lightvalve 13:747daba9cf59 818 REFERENCE_MODE = MODE_REF_COS_INC;
Lightvalve 13:747daba9cf59 819 CONTROL_MODE = MODE_JOINT_POSITION_TORQUE_CONTROL_PWM;
Lightvalve 13:747daba9cf59 820
Lightvalve 13:747daba9cf59 821 FINDHOME_POSITION = 0;
Lightvalve 13:747daba9cf59 822 FINDHOME_POSITION_OLD = 0;
Lightvalve 13:747daba9cf59 823 FINDHOME_VELOCITY = 0;
Lightvalve 13:747daba9cf59 824 cnt_findhome = 0;
Lightvalve 13:747daba9cf59 825 cnt_vel_findhome = 0;
Lightvalve 13:747daba9cf59 826 }
Lightvalve 13:747daba9cf59 827 break;
Lightvalve 13:747daba9cf59 828 }
Lightvalve 13:747daba9cf59 829
Lightvalve 13:747daba9cf59 830 case MODE_VALVE_GAIN_SETTING:
Lightvalve 13:747daba9cf59 831 {
Lightvalve 13:747daba9cf59 832 if (TMR2_COUNT_FLOWRATE == 0) {
Lightvalve 13:747daba9cf59 833 if (pos_plus_end == pos_minus_end) need_enc_init = true;
Lightvalve 13:747daba9cf59 834 else {
Lightvalve 13:747daba9cf59 835 VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
Lightvalve 13:747daba9cf59 836 temp_time = (int) (0.5 * (double) TMR_FREQ_5k);
Lightvalve 13:747daba9cf59 837 }
Lightvalve 13:747daba9cf59 838 }
Lightvalve 13:747daba9cf59 839 if (need_enc_init) {
Lightvalve 13:747daba9cf59 840 if (TMR2_COUNT_FLOWRATE < (int) (0.5 * (double) TMR_FREQ_5k)) {
Lightvalve 13:747daba9cf59 841 VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
Lightvalve 13:747daba9cf59 842 pos_plus_end = CUR_POSITION;
Lightvalve 13:747daba9cf59 843 } else if (TMR2_COUNT_FLOWRATE < TMR_FREQ_5k) {
Lightvalve 13:747daba9cf59 844 VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
Lightvalve 13:747daba9cf59 845 pos_minus_end = CUR_POSITION;
Lightvalve 13:747daba9cf59 846 } else if (TMR2_COUNT_FLOWRATE == TMR_FREQ_5k) {
Lightvalve 13:747daba9cf59 847 need_enc_init = false;
Lightvalve 13:747daba9cf59 848 check_vel_pos_init = (int) (0.9 * (double) (pos_plus_end - pos_minus_end));
Lightvalve 13:747daba9cf59 849 check_vel_pos_fin = (int) (0.95 * (double) (pos_plus_end - pos_minus_end));
Lightvalve 13:747daba9cf59 850 check_vel_pos_interv = check_vel_pos_fin - check_vel_pos_init;
Lightvalve 13:747daba9cf59 851 }
Lightvalve 13:747daba9cf59 852 temp_time = TMR_FREQ_5k;
Lightvalve 13:747daba9cf59 853 }
Lightvalve 13:747daba9cf59 854 TMR2_COUNT_FLOWRATE++;
Lightvalve 13:747daba9cf59 855 if (TMR2_COUNT_FLOWRATE > temp_time) {
Lightvalve 13:747daba9cf59 856 if (flag_flowrate % 2 == 0) { // (+)
Lightvalve 13:747daba9cf59 857 VALVE_VOLTAGE = 1. * (double) (flag_flowrate / 2 + 1);
Lightvalve 13:747daba9cf59 858 VALVE_PWM_RAW = VALVE_VOLTAGE / SUPPLY_VOLTAGE*PWM_RESOL;
Lightvalve 13:747daba9cf59 859 if (CUR_POSITION > (pos_minus_end + check_vel_pos_init) && CUR_POSITION < (pos_minus_end + check_vel_pos_fin)) {
Lightvalve 13:747daba9cf59 860 fl_temp_cnt++;
Lightvalve 13:747daba9cf59 861 } else if (CUR_POSITION >= (pos_minus_end + check_vel_pos_fin) && CUR_VELOCITY == 0) {
Lightvalve 13:747daba9cf59 862 VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873 * 0.5757 * (double) TMR_FREQ_5k / 10000.0 * (double) check_vel_pos_interv / (double) fl_temp_cnt / VALVE_VOLTAGE; // 0.9587=6*pi/65536*10000 0.5757=0.02525*0.02*0.0095*2*60*1000
Lightvalve 13:747daba9cf59 863 // VALVE_GAIN_LPM_PER_V[flag_flowrate] = (double) TMR_FREQ_10k * (double) check_vel_pos_interv / (double) fl_temp_cnt / VALVE_VOLTAGE; // PULSE/sec
Lightvalve 13:747daba9cf59 864 fl_temp_cnt2++;
Lightvalve 13:747daba9cf59 865 }
Lightvalve 13:747daba9cf59 866 } else if (flag_flowrate % 2 == 1) { // (-)
Lightvalve 13:747daba9cf59 867 VALVE_VOLTAGE = -1. * (double) (flag_flowrate / 2 + 1);
Lightvalve 13:747daba9cf59 868 VALVE_PWM_RAW = VALVE_VOLTAGE / SUPPLY_VOLTAGE*PWM_RESOL;
Lightvalve 13:747daba9cf59 869 if (CUR_POSITION < (pos_plus_end - check_vel_pos_init) && CUR_POSITION > (pos_plus_end - check_vel_pos_fin)) {
Lightvalve 13:747daba9cf59 870 fl_temp_cnt++;
Lightvalve 13:747daba9cf59 871 } else if (CUR_POSITION <= (pos_plus_end - check_vel_pos_fin) && CUR_VELOCITY == 0) {
Lightvalve 13:747daba9cf59 872 VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873 * 0.5757 * (double) TMR_FREQ_5k / 10000.0 * (double) check_vel_pos_interv / (double) fl_temp_cnt / (-VALVE_VOLTAGE);
Lightvalve 13:747daba9cf59 873 // VALVE_GAIN_LPM_PER_V[flag_flowrate] = (double) TMR_FREQ_10k * (double) check_vel_pos_interv / (double) fl_temp_cnt / (-VALVE_VOLTAGE); // PULSE/sec
Lightvalve 13:747daba9cf59 874 fl_temp_cnt2++;
Lightvalve 13:747daba9cf59 875 }
Lightvalve 13:747daba9cf59 876 }
Lightvalve 13:747daba9cf59 877 if (fl_temp_cnt2 == 100) {
Lightvalve 13:747daba9cf59 878 spi_eeprom_write(RID_VALVE_GAIN_PLUS_1 + flag_flowrate, (int16_t) (VALVE_GAIN_LPM_PER_V[flag_flowrate] * 100.0));
Lightvalve 13:747daba9cf59 879 cur_vel_sum = 0;
Lightvalve 13:747daba9cf59 880 fl_temp_cnt = 0;
Lightvalve 13:747daba9cf59 881 fl_temp_cnt2 = 0;
Lightvalve 13:747daba9cf59 882 flag_flowrate++;
Lightvalve 13:747daba9cf59 883 }
Lightvalve 13:747daba9cf59 884 if (flag_flowrate == 10) {
Lightvalve 13:747daba9cf59 885 VALVE_PWM_RAW = 0;
Lightvalve 13:747daba9cf59 886 flag_flowrate = 0;
Lightvalve 13:747daba9cf59 887 TMR2_COUNT_FLOWRATE = 0;
Lightvalve 13:747daba9cf59 888 valve_gain_repeat_cnt++;
Lightvalve 13:747daba9cf59 889 if (valve_gain_repeat_cnt >= 1) {
Lightvalve 13:747daba9cf59 890 CONTROL_MODE = MODE_NO_ACT;
Lightvalve 13:747daba9cf59 891 valve_gain_repeat_cnt = 0;
Lightvalve 13:747daba9cf59 892 }
Lightvalve 13:747daba9cf59 893
Lightvalve 13:747daba9cf59 894 }
Lightvalve 13:747daba9cf59 895 break;
Lightvalve 13:747daba9cf59 896 }
Lightvalve 13:747daba9cf59 897
Lightvalve 13:747daba9cf59 898 }
Lightvalve 13:747daba9cf59 899
Lightvalve 13:747daba9cf59 900 case MODE_PRESSURE_SENSOR_NULLING:
Lightvalve 13:747daba9cf59 901 {
Lightvalve 13:747daba9cf59 902 // DAC Voltage reference set
Lightvalve 13:747daba9cf59 903 if (TMR2_COUNT_PRES_NULL < TMR_FREQ_5k * 2) {
Lightvalve 13:747daba9cf59 904 CUR_PRES_A_sum += CUR_PRES_A;
Lightvalve 13:747daba9cf59 905 CUR_PRES_B_sum += CUR_PRES_B;
Lightvalve 13:747daba9cf59 906
Lightvalve 13:747daba9cf59 907 if (TMR2_COUNT_PRES_NULL % 10 == 0) {
Lightvalve 13:747daba9cf59 908 CUR_PRES_A_mean = CUR_PRES_A_sum / 10.0;
Lightvalve 13:747daba9cf59 909 CUR_PRES_B_mean = CUR_PRES_B_sum / 10.0;
Lightvalve 13:747daba9cf59 910 CUR_PRES_A_sum = 0;
Lightvalve 13:747daba9cf59 911 CUR_PRES_B_sum = 0;
Lightvalve 13:747daba9cf59 912
Lightvalve 13:747daba9cf59 913 double VREF_NullingGain = 0.0003;
Lightvalve 13:747daba9cf59 914 PRES_A_VREF -= VREF_NullingGain * (PRES_A_NULL - CUR_PRES_A_mean);
Lightvalve 13:747daba9cf59 915 PRES_B_VREF -= VREF_NullingGain * (PRES_B_NULL - CUR_PRES_B_mean);
Lightvalve 13:747daba9cf59 916
Lightvalve 13:747daba9cf59 917 if (PRES_A_VREF > 3.3) PRES_A_VREF = 3.3;
Lightvalve 13:747daba9cf59 918 if (PRES_A_VREF < 0) PRES_A_VREF = 0;
Lightvalve 13:747daba9cf59 919 if (PRES_B_VREF > 3.3) PRES_B_VREF = 3.3;
Lightvalve 13:747daba9cf59 920 if (PRES_B_VREF < 0) PRES_B_VREF = 0;
Lightvalve 13:747daba9cf59 921
Lightvalve 13:747daba9cf59 922 spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0));
Lightvalve 13:747daba9cf59 923 spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0));
Lightvalve 13:747daba9cf59 924 }
Lightvalve 13:747daba9cf59 925 } else {
Lightvalve 13:747daba9cf59 926 CONTROL_MODE = MODE_NO_ACT;
Lightvalve 13:747daba9cf59 927 TMR2_COUNT_PRES_NULL = 0;
Lightvalve 13:747daba9cf59 928 CUR_PRES_A_sum = 0;
Lightvalve 13:747daba9cf59 929 CUR_PRES_B_sum = 0;
Lightvalve 13:747daba9cf59 930 CUR_PRES_A_mean = 0;
Lightvalve 13:747daba9cf59 931 CUR_PRES_B_mean = 0;
Lightvalve 13:747daba9cf59 932
Lightvalve 13:747daba9cf59 933 spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0));
Lightvalve 13:747daba9cf59 934 spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0));
Lightvalve 13:747daba9cf59 935 }
Lightvalve 13:747daba9cf59 936 TMR2_COUNT_PRES_NULL++;
Lightvalve 13:747daba9cf59 937 break;
Lightvalve 13:747daba9cf59 938 }
Lightvalve 13:747daba9cf59 939
Lightvalve 13:747daba9cf59 940 case MODE_PRESSURE_SENSOR_CALIB:
Lightvalve 13:747daba9cf59 941 {
Lightvalve 13:747daba9cf59 942 if (TMR2_COUNT_PRES_CALIB < 2 * TMR_FREQ_5k) {
Lightvalve 13:747daba9cf59 943 VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL / 5.;
Lightvalve 13:747daba9cf59 944 if (TMR2_COUNT_PRES_CALIB >= TMR_FREQ_5k) {
Lightvalve 13:747daba9cf59 945 CUR_PRES_A_sum += CUR_PRES_A;
Lightvalve 13:747daba9cf59 946 }
Lightvalve 13:747daba9cf59 947 } else if (TMR2_COUNT_PRES_CALIB < 4 * TMR_FREQ_5k) {
Lightvalve 13:747daba9cf59 948 VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL / 5.;
Lightvalve 13:747daba9cf59 949 if (TMR2_COUNT_PRES_CALIB >= 3 * TMR_FREQ_5k) {
Lightvalve 13:747daba9cf59 950 CUR_PRES_B_sum += CUR_PRES_B;
Lightvalve 13:747daba9cf59 951 }
Lightvalve 13:747daba9cf59 952 } else {
Lightvalve 13:747daba9cf59 953 CONTROL_MODE = MODE_NO_ACT;
Lightvalve 13:747daba9cf59 954 TMR2_COUNT_PRES_CALIB = 0;
Lightvalve 13:747daba9cf59 955 VALVE_PWM_RAW = 0;
Lightvalve 13:747daba9cf59 956 PRES_SENSOR_A_PULSE_PER_BAR = CUR_PRES_A_sum / ((double) TMR_FREQ_5k - 1.) - PRES_A_NULL;
Lightvalve 13:747daba9cf59 957 PRES_SENSOR_A_PULSE_PER_BAR = PRES_SENSOR_A_PULSE_PER_BAR / ((double) PRES_SUPPLY - 1.);
Lightvalve 13:747daba9cf59 958 PRES_SENSOR_B_PULSE_PER_BAR = CUR_PRES_B_sum / ((double) TMR_FREQ_5k - 1.) - PRES_B_NULL;
Lightvalve 13:747daba9cf59 959 PRES_SENSOR_B_PULSE_PER_BAR = PRES_SENSOR_B_PULSE_PER_BAR / ((double) PRES_SUPPLY - 1.);
Lightvalve 13:747daba9cf59 960 CUR_PRES_A_sum = 0;
Lightvalve 13:747daba9cf59 961 CUR_PRES_B_sum = 0;
Lightvalve 13:747daba9cf59 962 CUR_PRES_A_mean = 0;
Lightvalve 13:747daba9cf59 963 CUR_PRES_B_mean = 0;
Lightvalve 13:747daba9cf59 964
Lightvalve 13:747daba9cf59 965 spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0));
Lightvalve 13:747daba9cf59 966 spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0));
Lightvalve 13:747daba9cf59 967 }
Lightvalve 13:747daba9cf59 968 TMR2_COUNT_PRES_CALIB++;
Lightvalve 13:747daba9cf59 969 break;
Lightvalve 13:747daba9cf59 970 }
Lightvalve 13:747daba9cf59 971
Lightvalve 13:747daba9cf59 972 case MODE_ROTARY_FRICTION_TUNING:
Lightvalve 13:747daba9cf59 973 {
Lightvalve 13:747daba9cf59 974 if (TMR2_COUNT_ROTARY_FRIC_TUNE % (5 * TMR_FREQ_5k) == 0) freq_fric_tune = 4 + 3. * sin(2 * 3.14159 * 0.5 * TMR2_COUNT_ROTARY_FRIC_TUNE * 0.0001 * 0.05);
Lightvalve 13:747daba9cf59 975 VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL * sin(2 * 3.14159 * freq_fric_tune * TMR2_COUNT_ROTARY_FRIC_TUNE * 0.0001);
Lightvalve 13:747daba9cf59 976 if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL;
Lightvalve 13:747daba9cf59 977 else VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
Lightvalve 13:747daba9cf59 978 TMR2_COUNT_ROTARY_FRIC_TUNE++;
Lightvalve 13:747daba9cf59 979 if (TMR2_COUNT_ROTARY_FRIC_TUNE > TUNING_TIME * TMR_FREQ_5k) {
Lightvalve 13:747daba9cf59 980 TMR2_COUNT_ROTARY_FRIC_TUNE = 0;
Lightvalve 13:747daba9cf59 981 VALVE_PWM_RAW = 0;
Lightvalve 13:747daba9cf59 982 CONTROL_MODE = MODE_NO_ACT;
Lightvalve 13:747daba9cf59 983 }
Lightvalve 13:747daba9cf59 984 break;
Lightvalve 13:747daba9cf59 985 }
Lightvalve 13:747daba9cf59 986
Lightvalve 13:747daba9cf59 987 case MODE_DDV_POS_VS_PWM_ID:
Lightvalve 13:747daba9cf59 988 {
Lightvalve 13:747daba9cf59 989 VALVE_ID_timer = VALVE_ID_timer + 1;
Lightvalve 13:747daba9cf59 990
Lightvalve 13:747daba9cf59 991 if(VALVE_ID_timer < TMR_FREQ_5k*1)
Lightvalve 13:747daba9cf59 992 {
Lightvalve 13:747daba9cf59 993 VALVE_PWM_RAW = 3000 * sin(2*3.14*VALVE_ID_timer/TMR_FREQ_5k * 100);
Lightvalve 13:747daba9cf59 994 }
Lightvalve 13:747daba9cf59 995 else if(VALVE_ID_timer < TMR_FREQ_5k*2)
Lightvalve 13:747daba9cf59 996 {
Lightvalve 13:747daba9cf59 997 VALVE_PWM_RAW = 1000*(ID_index_array[ID_index]);
Lightvalve 13:747daba9cf59 998 }
Lightvalve 13:747daba9cf59 999 else if(VALVE_ID_timer == TMR_FREQ_5k*2)
Lightvalve 13:747daba9cf59 1000 {
Lightvalve 13:747daba9cf59 1001 VALVE_POS_TMP = 0;
Lightvalve 13:747daba9cf59 1002 data_num = 0;
Lightvalve 13:747daba9cf59 1003 }
Lightvalve 13:747daba9cf59 1004 else if(VALVE_ID_timer < TMR_FREQ_5k*3)
Lightvalve 13:747daba9cf59 1005 {
Lightvalve 13:747daba9cf59 1006 data_num = data_num + 1;
Lightvalve 13:747daba9cf59 1007 VALVE_POS_TMP = VALVE_POS_TMP + value;
Lightvalve 13:747daba9cf59 1008 }
Lightvalve 13:747daba9cf59 1009 else if(VALVE_ID_timer == TMR_FREQ_5k*3)
Lightvalve 13:747daba9cf59 1010 {
Lightvalve 13:747daba9cf59 1011 VALVE_POS_AVG[ID_index] = VALVE_POS_TMP / data_num;
Lightvalve 13:747daba9cf59 1012 VALVE_ID_timer = 0;
Lightvalve 13:747daba9cf59 1013 ID_index= ID_index +1;
Lightvalve 13:747daba9cf59 1014 }
Lightvalve 13:747daba9cf59 1015
Lightvalve 13:747daba9cf59 1016 if(ID_index>=18)
Lightvalve 13:747daba9cf59 1017 {
Lightvalve 13:747daba9cf59 1018 int i;
Lightvalve 13:747daba9cf59 1019 VALVE_POS_AVG_OLD = VALVE_POS_AVG[0];
Lightvalve 13:747daba9cf59 1020 for(i=0;i<18;i++)
Lightvalve 13:747daba9cf59 1021 {
Lightvalve 13:747daba9cf59 1022 VALVE_POS_VS_PWM[i] = (int16_t) (VALVE_POS_AVG[i]);
Lightvalve 13:747daba9cf59 1023 spi_eeprom_write(RID_VALVE_POS_VS_PWM_0 + i, (int16_t) (VALVE_POS_AVG[i]));
Lightvalve 13:747daba9cf59 1024 if(VALVE_POS_AVG[i] > VALVE_POS_AVG_OLD)
Lightvalve 13:747daba9cf59 1025 {
Lightvalve 13:747daba9cf59 1026 VALVE_MAX_POS = VALVE_POS_AVG[i];
Lightvalve 13:747daba9cf59 1027 VALVE_POS_AVG_OLD = VALVE_MAX_POS;
Lightvalve 13:747daba9cf59 1028 }
Lightvalve 13:747daba9cf59 1029 else if(VALVE_POS_AVG[i] < VALVE_POS_AVG_OLD)
Lightvalve 13:747daba9cf59 1030 {
Lightvalve 13:747daba9cf59 1031 VALVE_MIN_POS = VALVE_POS_AVG[i];
Lightvalve 13:747daba9cf59 1032 VALVE_POS_AVG_OLD = VALVE_MIN_POS;
Lightvalve 13:747daba9cf59 1033 }
Lightvalve 13:747daba9cf59 1034 }
Lightvalve 13:747daba9cf59 1035 spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) (VALVE_MAX_POS));
Lightvalve 13:747daba9cf59 1036 spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) (VALVE_MIN_POS));
Lightvalve 13:747daba9cf59 1037 CAN_TX_PRES((int16_t) (VALVE_MAX_POS), (int16_t) (VALVE_MIN_POS));
Lightvalve 13:747daba9cf59 1038 ID_index = 0;
Lightvalve 13:747daba9cf59 1039 CONTROL_MODE = MODE_NO_ACT;
Lightvalve 13:747daba9cf59 1040 }
Lightvalve 13:747daba9cf59 1041
Lightvalve 13:747daba9cf59 1042
Lightvalve 13:747daba9cf59 1043 break;
Lightvalve 13:747daba9cf59 1044 }
Lightvalve 13:747daba9cf59 1045
Lightvalve 13:747daba9cf59 1046 case MODE_DDV_DEADZONE_AND_CENTER:
Lightvalve 13:747daba9cf59 1047 {
Lightvalve 13:747daba9cf59 1048
Lightvalve 13:747daba9cf59 1049 VALVE_DZ_timer = VALVE_DZ_timer + 1;
Lightvalve 13:747daba9cf59 1050 if(first_check == 0)
Lightvalve 13:747daba9cf59 1051 {
Lightvalve 13:747daba9cf59 1052 if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1053 {
Lightvalve 13:747daba9cf59 1054 VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
Lightvalve 13:747daba9cf59 1055 }
Lightvalve 13:747daba9cf59 1056 else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1057 {
Lightvalve 13:747daba9cf59 1058 VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
Lightvalve 13:747daba9cf59 1059 pos_plus_end = CUR_POSITION;
Lightvalve 13:747daba9cf59 1060 }
Lightvalve 13:747daba9cf59 1061 else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1062 {
Lightvalve 13:747daba9cf59 1063 VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
Lightvalve 13:747daba9cf59 1064 }
Lightvalve 13:747daba9cf59 1065 else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1066 {
Lightvalve 13:747daba9cf59 1067 VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
Lightvalve 13:747daba9cf59 1068 pos_minus_end = CUR_POSITION;
Lightvalve 13:747daba9cf59 1069 }
Lightvalve 13:747daba9cf59 1070 else if(VALVE_DZ_timer < (int) (3.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1071 {
Lightvalve 13:747daba9cf59 1072 VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION);
Lightvalve 13:747daba9cf59 1073 }
Lightvalve 13:747daba9cf59 1074 else if(VALVE_DZ_timer < (int) (4.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1075 {
Lightvalve 13:747daba9cf59 1076 VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION);
Lightvalve 13:747daba9cf59 1077 data_num = data_num + 1;
Lightvalve 13:747daba9cf59 1078 VALVE_POS_TMP = VALVE_POS_TMP + value;
Lightvalve 13:747daba9cf59 1079 }
Lightvalve 13:747daba9cf59 1080 else if(VALVE_DZ_timer == (int) (4.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1081 {
Lightvalve 13:747daba9cf59 1082 VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION);
Lightvalve 13:747daba9cf59 1083 DDV_POS_AVG = VALVE_POS_TMP / data_num;
Lightvalve 13:747daba9cf59 1084 START_POS = CUR_POSITION;
Lightvalve 13:747daba9cf59 1085 CAN_TX_PRES((int16_t) (DDV_POS_AVG), (int16_t) (data_num));
Lightvalve 13:747daba9cf59 1086 VALVE_POS_TMP = 0;
Lightvalve 13:747daba9cf59 1087 data_num = 0;
Lightvalve 13:747daba9cf59 1088
Lightvalve 13:747daba9cf59 1089 }
Lightvalve 13:747daba9cf59 1090 else if(VALVE_DZ_timer < (int) (5.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1091 {
Lightvalve 13:747daba9cf59 1092 Ref_Valve_Pos = DDV_POS_AVG;
Lightvalve 13:747daba9cf59 1093 VALVE_POS_CONTROL(Ref_Valve_Pos);
Lightvalve 13:747daba9cf59 1094 //CAN_TX_PRES((int16_t) (VALVE_MAX_POS), (int16_t) (VALVE_MIN_POS));
Lightvalve 13:747daba9cf59 1095
Lightvalve 13:747daba9cf59 1096 }
Lightvalve 13:747daba9cf59 1097 else if(VALVE_DZ_timer < (int) (6.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1098 {
Lightvalve 13:747daba9cf59 1099 Ref_Valve_Pos = DDV_POS_AVG;
Lightvalve 13:747daba9cf59 1100 VALVE_POS_CONTROL(Ref_Valve_Pos);
Lightvalve 13:747daba9cf59 1101 if(CUR_VELOCITY >= 0)
Lightvalve 13:747daba9cf59 1102 VEL_POINT = VEL_POINT + 1;
Lightvalve 13:747daba9cf59 1103 else
Lightvalve 13:747daba9cf59 1104 VEL_POINT = VEL_POINT - 1;
Lightvalve 13:747daba9cf59 1105 }
Lightvalve 13:747daba9cf59 1106 else if(VALVE_DZ_timer == (int) (6.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1107 {
Lightvalve 13:747daba9cf59 1108 Ref_Valve_Pos = DDV_POS_AVG;
Lightvalve 13:747daba9cf59 1109 VALVE_POS_CONTROL(Ref_Valve_Pos);
Lightvalve 13:747daba9cf59 1110 FINAL_POS = CUR_POSITION;
Lightvalve 13:747daba9cf59 1111 // if(VEL_POINT >= 0)
Lightvalve 13:747daba9cf59 1112 // {
Lightvalve 13:747daba9cf59 1113 // DZ_case = 1;
Lightvalve 13:747daba9cf59 1114 // }
Lightvalve 13:747daba9cf59 1115 // else
Lightvalve 13:747daba9cf59 1116 // {
Lightvalve 13:747daba9cf59 1117 // DZ_case = -1;
Lightvalve 13:747daba9cf59 1118 // }
Lightvalve 13:747daba9cf59 1119 // if(abs(FINAL_POS - START_POS) < 100)
Lightvalve 13:747daba9cf59 1120 // {
Lightvalve 13:747daba9cf59 1121 // DZ_case = 0;
Lightvalve 13:747daba9cf59 1122 // }
Lightvalve 13:747daba9cf59 1123
Lightvalve 13:747daba9cf59 1124 if((FINAL_POS - START_POS)>100)
Lightvalve 13:747daba9cf59 1125 {
Lightvalve 13:747daba9cf59 1126 DZ_case = 1;
Lightvalve 13:747daba9cf59 1127 }
Lightvalve 13:747daba9cf59 1128 else if((FINAL_POS - START_POS)<-100)
Lightvalve 13:747daba9cf59 1129 {
Lightvalve 13:747daba9cf59 1130 DZ_case = -1;
Lightvalve 13:747daba9cf59 1131 }
Lightvalve 13:747daba9cf59 1132 else
Lightvalve 13:747daba9cf59 1133 {
Lightvalve 13:747daba9cf59 1134 DZ_case = 0;
Lightvalve 13:747daba9cf59 1135 }
Lightvalve 13:747daba9cf59 1136
Lightvalve 13:747daba9cf59 1137 VEL_POINT = 0;
Lightvalve 13:747daba9cf59 1138 first_check = 1;
Lightvalve 13:747daba9cf59 1139 DZ_DIRECTION = 1;
Lightvalve 13:747daba9cf59 1140 VALVE_DZ_timer = 0;
Lightvalve 13:747daba9cf59 1141 Ref_Valve_Pos_Old = DDV_POS_AVG;
Lightvalve 13:747daba9cf59 1142 DZ_NUM = 1;
Lightvalve 13:747daba9cf59 1143 DZ_index = 1;
Lightvalve 13:747daba9cf59 1144 CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (DZ_NUM));
Lightvalve 13:747daba9cf59 1145
Lightvalve 13:747daba9cf59 1146 }
Lightvalve 13:747daba9cf59 1147 }
Lightvalve 13:747daba9cf59 1148 else
Lightvalve 13:747daba9cf59 1149 {
Lightvalve 13:747daba9cf59 1150 if((DZ_case == -1 && DZ_NUM == 1) | (DZ_case == 1 && DZ_NUM == 1))
Lightvalve 13:747daba9cf59 1151 {
Lightvalve 13:747daba9cf59 1152 if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1153 {
Lightvalve 13:747daba9cf59 1154 VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION);
Lightvalve 13:747daba9cf59 1155 }
Lightvalve 13:747daba9cf59 1156 else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1157 {
Lightvalve 13:747daba9cf59 1158 START_POS = CUR_POSITION;
Lightvalve 13:747daba9cf59 1159 }
Lightvalve 13:747daba9cf59 1160 else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1161 {
Lightvalve 13:747daba9cf59 1162 Ref_Valve_Pos = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index;
Lightvalve 13:747daba9cf59 1163 if(Ref_Valve_Pos <= VALVE_MIN_POS)
Lightvalve 13:747daba9cf59 1164 {
Lightvalve 13:747daba9cf59 1165 Ref_Valve_Pos = VALVE_MIN_POS;
Lightvalve 13:747daba9cf59 1166 }
Lightvalve 13:747daba9cf59 1167 else if(Ref_Valve_Pos >= VALVE_MAX_POS)
Lightvalve 13:747daba9cf59 1168 {
Lightvalve 13:747daba9cf59 1169 Ref_Valve_Pos = VALVE_MAX_POS;
Lightvalve 13:747daba9cf59 1170 }
Lightvalve 13:747daba9cf59 1171 VALVE_POS_CONTROL(Ref_Valve_Pos);
Lightvalve 13:747daba9cf59 1172 if(CUR_VELOCITY >= 0)
Lightvalve 13:747daba9cf59 1173 VEL_POINT = VEL_POINT + 1;
Lightvalve 13:747daba9cf59 1174 else
Lightvalve 13:747daba9cf59 1175 VEL_POINT = VEL_POINT - 1;
Lightvalve 13:747daba9cf59 1176 }
Lightvalve 13:747daba9cf59 1177 else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1178 {
Lightvalve 13:747daba9cf59 1179 Ref_Valve_Pos_Old = Ref_Valve_Pos;
Lightvalve 13:747daba9cf59 1180 FINAL_POS = CUR_POSITION;
Lightvalve 13:747daba9cf59 1181 // if(VEL_POINT >= 0)
Lightvalve 13:747daba9cf59 1182 // DZ_DIRECTION = 1 * DZ_case;
Lightvalve 13:747daba9cf59 1183 // else
Lightvalve 13:747daba9cf59 1184 // DZ_DIRECTION = -1 * DZ_case;
Lightvalve 13:747daba9cf59 1185 // if(abs(FINAL_POS - START_POS) < 100)
Lightvalve 13:747daba9cf59 1186 // DZ_DIRECTION = 1 * DZ_case;
Lightvalve 13:747daba9cf59 1187
Lightvalve 13:747daba9cf59 1188 if((FINAL_POS - START_POS)>100)
Lightvalve 13:747daba9cf59 1189 {
Lightvalve 13:747daba9cf59 1190 DZ_DIRECTION = 1 * DZ_case;
Lightvalve 13:747daba9cf59 1191 }
Lightvalve 13:747daba9cf59 1192 else if((FINAL_POS - START_POS)<-100)
Lightvalve 13:747daba9cf59 1193 {
Lightvalve 13:747daba9cf59 1194 DZ_DIRECTION = -1 * DZ_case;
Lightvalve 13:747daba9cf59 1195 }
Lightvalve 13:747daba9cf59 1196 else
Lightvalve 13:747daba9cf59 1197 {
Lightvalve 13:747daba9cf59 1198 DZ_DIRECTION = 1 * DZ_case;
Lightvalve 13:747daba9cf59 1199 }
Lightvalve 13:747daba9cf59 1200
Lightvalve 13:747daba9cf59 1201 VEL_POINT = 0;
Lightvalve 13:747daba9cf59 1202
Lightvalve 13:747daba9cf59 1203 VALVE_DZ_timer = 0;
Lightvalve 13:747daba9cf59 1204 DZ_index= DZ_index *2;
Lightvalve 13:747daba9cf59 1205 if(DZ_index >= 128)
Lightvalve 13:747daba9cf59 1206 {
Lightvalve 13:747daba9cf59 1207 FIRST_DZ = Ref_Valve_Pos;
Lightvalve 13:747daba9cf59 1208 DZ_NUM = 2;
Lightvalve 13:747daba9cf59 1209 Ref_Valve_Pos_Old = FIRST_DZ;
Lightvalve 13:747daba9cf59 1210 DZ_index = 1;
Lightvalve 13:747daba9cf59 1211 DZ_DIRECTION = 1;
Lightvalve 13:747daba9cf59 1212 }
Lightvalve 13:747daba9cf59 1213
Lightvalve 13:747daba9cf59 1214 //CAN_TX_PRES((int16_t) (1), (int16_t) (DZ_index));
Lightvalve 13:747daba9cf59 1215 }
Lightvalve 13:747daba9cf59 1216 }
Lightvalve 13:747daba9cf59 1217 else if((DZ_case == -1 && DZ_NUM == 2) | (DZ_case == 1 && DZ_NUM == 2))
Lightvalve 13:747daba9cf59 1218 {
Lightvalve 13:747daba9cf59 1219 if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1220 {
Lightvalve 13:747daba9cf59 1221 VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION);
Lightvalve 13:747daba9cf59 1222 }
Lightvalve 13:747daba9cf59 1223 else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1224 {
Lightvalve 13:747daba9cf59 1225 START_POS = CUR_POSITION;
Lightvalve 13:747daba9cf59 1226 }
Lightvalve 13:747daba9cf59 1227 else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1228 {
Lightvalve 13:747daba9cf59 1229 Ref_Valve_Pos = Ref_Valve_Pos_Old + DZ_DIRECTION + 64 / DZ_index;
Lightvalve 13:747daba9cf59 1230 if(Ref_Valve_Pos <= VALVE_MIN_POS)
Lightvalve 13:747daba9cf59 1231 {
Lightvalve 13:747daba9cf59 1232 Ref_Valve_Pos = VALVE_MIN_POS;
Lightvalve 13:747daba9cf59 1233 }
Lightvalve 13:747daba9cf59 1234 else if(Ref_Valve_Pos >= VALVE_MAX_POS)
Lightvalve 13:747daba9cf59 1235 {
Lightvalve 13:747daba9cf59 1236 Ref_Valve_Pos = VALVE_MAX_POS;
Lightvalve 13:747daba9cf59 1237 }
Lightvalve 13:747daba9cf59 1238 VALVE_POS_CONTROL(Ref_Valve_Pos);
Lightvalve 13:747daba9cf59 1239 if(CUR_VELOCITY >= 0)
Lightvalve 13:747daba9cf59 1240 VEL_POINT = VEL_POINT + 1;
Lightvalve 13:747daba9cf59 1241 else
Lightvalve 13:747daba9cf59 1242 VEL_POINT = VEL_POINT - 1;
Lightvalve 13:747daba9cf59 1243 }
Lightvalve 13:747daba9cf59 1244 else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1245 {
Lightvalve 13:747daba9cf59 1246 Ref_Valve_Pos_Old = Ref_Valve_Pos;
Lightvalve 13:747daba9cf59 1247 FINAL_POS = CUR_POSITION;
Lightvalve 13:747daba9cf59 1248 // if(VEL_POINT >= 0)
Lightvalve 13:747daba9cf59 1249 // DZ_DIRECTION = 1 * DZ_case;
Lightvalve 13:747daba9cf59 1250 // else
Lightvalve 13:747daba9cf59 1251 // DZ_DIRECTION = -1 * DZ_case;
Lightvalve 13:747daba9cf59 1252 // if(abs(FINAL_POS - START_POS) < 100)
Lightvalve 13:747daba9cf59 1253 // DZ_DIRECTION = -1 * DZ_case;
Lightvalve 13:747daba9cf59 1254
Lightvalve 13:747daba9cf59 1255 if((FINAL_POS - START_POS)>100)
Lightvalve 13:747daba9cf59 1256 {
Lightvalve 13:747daba9cf59 1257 DZ_DIRECTION = -1;
Lightvalve 13:747daba9cf59 1258 }
Lightvalve 13:747daba9cf59 1259 else if((FINAL_POS - START_POS)<-100)
Lightvalve 13:747daba9cf59 1260 {
Lightvalve 13:747daba9cf59 1261 DZ_DIRECTION = 1;
Lightvalve 13:747daba9cf59 1262 }
Lightvalve 13:747daba9cf59 1263 else
Lightvalve 13:747daba9cf59 1264 {
Lightvalve 13:747daba9cf59 1265 DZ_DIRECTION = 1;
Lightvalve 13:747daba9cf59 1266 }
Lightvalve 13:747daba9cf59 1267
Lightvalve 13:747daba9cf59 1268 VEL_POINT = 0;
Lightvalve 13:747daba9cf59 1269
Lightvalve 13:747daba9cf59 1270
Lightvalve 13:747daba9cf59 1271 VALVE_DZ_timer = 0;
Lightvalve 13:747daba9cf59 1272 DZ_index= DZ_index *2;
Lightvalve 13:747daba9cf59 1273 if(DZ_index >= 128)
Lightvalve 13:747daba9cf59 1274 {
Lightvalve 13:747daba9cf59 1275 SECOND_DZ = Ref_Valve_Pos;
Lightvalve 13:747daba9cf59 1276 DDV_CENTER = (int) (0.5 * (double) (FIRST_DZ) + 0.5 * (double) (SECOND_DZ));
Lightvalve 13:747daba9cf59 1277 first_check = 0;
Lightvalve 13:747daba9cf59 1278 spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ);
Lightvalve 13:747daba9cf59 1279 spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ);
Lightvalve 13:747daba9cf59 1280 spi_eeprom_write(RID_DDV_CNETER, DDV_CENTER);
Lightvalve 13:747daba9cf59 1281 //CAN_TX_PRES((int16_t) FIRST_DZ, (int16_t) SECOND_DZ);
Lightvalve 13:747daba9cf59 1282 CONTROL_MODE = MODE_NO_ACT;
Lightvalve 13:747daba9cf59 1283 DZ_index = 1;
Lightvalve 13:747daba9cf59 1284 }
Lightvalve 13:747daba9cf59 1285
Lightvalve 13:747daba9cf59 1286 CAN_TX_PRES((int16_t) (2), (int16_t) (DZ_index));
Lightvalve 13:747daba9cf59 1287 }
Lightvalve 13:747daba9cf59 1288 }
Lightvalve 13:747daba9cf59 1289 else if(DZ_case == 0 && DZ_NUM ==1)
Lightvalve 13:747daba9cf59 1290 {
Lightvalve 13:747daba9cf59 1291 if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1292 {
Lightvalve 13:747daba9cf59 1293 VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01* (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION);
Lightvalve 13:747daba9cf59 1294 }
Lightvalve 13:747daba9cf59 1295 else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1296 {
Lightvalve 13:747daba9cf59 1297 START_POS = CUR_POSITION;
Lightvalve 13:747daba9cf59 1298 }
Lightvalve 13:747daba9cf59 1299 else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1300 {
Lightvalve 13:747daba9cf59 1301 Ref_Valve_Pos = Ref_Valve_Pos_Old - DZ_DIRECTION * 64 / DZ_index;
Lightvalve 13:747daba9cf59 1302 if(Ref_Valve_Pos <= VALVE_MIN_POS)
Lightvalve 13:747daba9cf59 1303 {
Lightvalve 13:747daba9cf59 1304 Ref_Valve_Pos = VALVE_MIN_POS;
Lightvalve 13:747daba9cf59 1305 }
Lightvalve 13:747daba9cf59 1306 else if(Ref_Valve_Pos >= VALVE_MAX_POS)
Lightvalve 13:747daba9cf59 1307 {
Lightvalve 13:747daba9cf59 1308 Ref_Valve_Pos = VALVE_MAX_POS;
Lightvalve 13:747daba9cf59 1309 }
Lightvalve 13:747daba9cf59 1310 VALVE_POS_CONTROL(Ref_Valve_Pos);
Lightvalve 13:747daba9cf59 1311 if(CUR_VELOCITY >= 0)
Lightvalve 13:747daba9cf59 1312 VEL_POINT = VEL_POINT + 1;
Lightvalve 13:747daba9cf59 1313 else
Lightvalve 13:747daba9cf59 1314 VEL_POINT = VEL_POINT - 1;
Lightvalve 13:747daba9cf59 1315 }
Lightvalve 13:747daba9cf59 1316 else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1317 {
Lightvalve 13:747daba9cf59 1318 Ref_Valve_Pos_Old = Ref_Valve_Pos;
Lightvalve 13:747daba9cf59 1319 FINAL_POS = CUR_POSITION;
Lightvalve 13:747daba9cf59 1320 // if(VEL_POINT >= 0)
Lightvalve 13:747daba9cf59 1321 // DZ_DIRECTION = 1;
Lightvalve 13:747daba9cf59 1322 // else
Lightvalve 13:747daba9cf59 1323 // DZ_DIRECTION = -1;
Lightvalve 13:747daba9cf59 1324 // if(abs(FINAL_POS - START_POS) < 100)
Lightvalve 13:747daba9cf59 1325 // DZ_DIRECTION = 1;
Lightvalve 13:747daba9cf59 1326
Lightvalve 13:747daba9cf59 1327 if((FINAL_POS - START_POS)>100)
Lightvalve 13:747daba9cf59 1328 {
Lightvalve 13:747daba9cf59 1329 DZ_DIRECTION = 1;
Lightvalve 13:747daba9cf59 1330 }
Lightvalve 13:747daba9cf59 1331 else if((FINAL_POS - START_POS)<-100)
Lightvalve 13:747daba9cf59 1332 {
Lightvalve 13:747daba9cf59 1333 DZ_DIRECTION = -1;
Lightvalve 13:747daba9cf59 1334 }
Lightvalve 13:747daba9cf59 1335 else
Lightvalve 13:747daba9cf59 1336 {
Lightvalve 13:747daba9cf59 1337 DZ_DIRECTION = 1;
Lightvalve 13:747daba9cf59 1338 }
Lightvalve 13:747daba9cf59 1339
Lightvalve 13:747daba9cf59 1340 VEL_POINT = 0;
Lightvalve 13:747daba9cf59 1341 //CAN_TX_PRES((int16_t) (7), (int16_t) (DZ_DIRECTION));
Lightvalve 13:747daba9cf59 1342 VALVE_DZ_timer = 0;
Lightvalve 13:747daba9cf59 1343 DZ_index= DZ_index *2;
Lightvalve 13:747daba9cf59 1344 if(DZ_index >= 128)
Lightvalve 13:747daba9cf59 1345 {
Lightvalve 13:747daba9cf59 1346 FIRST_DZ = Ref_Valve_Pos;
Lightvalve 13:747daba9cf59 1347 DZ_NUM = 2;
Lightvalve 13:747daba9cf59 1348 Ref_Valve_Pos_Old = FIRST_DZ;
Lightvalve 13:747daba9cf59 1349 DZ_index = 1;
Lightvalve 13:747daba9cf59 1350 DZ_DIRECTION = 1;
Lightvalve 13:747daba9cf59 1351 }
Lightvalve 13:747daba9cf59 1352 }
Lightvalve 13:747daba9cf59 1353 }
Lightvalve 13:747daba9cf59 1354 else
Lightvalve 13:747daba9cf59 1355 {
Lightvalve 13:747daba9cf59 1356 if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1357 {
Lightvalve 13:747daba9cf59 1358 VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01* (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION);
Lightvalve 13:747daba9cf59 1359 }
Lightvalve 13:747daba9cf59 1360 else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1361 {
Lightvalve 13:747daba9cf59 1362 START_POS = CUR_POSITION;
Lightvalve 13:747daba9cf59 1363 }
Lightvalve 13:747daba9cf59 1364 else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1365 {
Lightvalve 13:747daba9cf59 1366 Ref_Valve_Pos = Ref_Valve_Pos_Old + DZ_DIRECTION * 64 / DZ_index;
Lightvalve 13:747daba9cf59 1367 if(Ref_Valve_Pos <= VALVE_MIN_POS)
Lightvalve 13:747daba9cf59 1368 {
Lightvalve 13:747daba9cf59 1369 Ref_Valve_Pos = VALVE_MIN_POS;
Lightvalve 13:747daba9cf59 1370 }
Lightvalve 13:747daba9cf59 1371 else if(Ref_Valve_Pos > VALVE_MAX_POS)
Lightvalve 13:747daba9cf59 1372 {
Lightvalve 13:747daba9cf59 1373 Ref_Valve_Pos = VALVE_MAX_POS - 1;
Lightvalve 13:747daba9cf59 1374 }
Lightvalve 13:747daba9cf59 1375 VALVE_POS_CONTROL(Ref_Valve_Pos);
Lightvalve 13:747daba9cf59 1376 if(CUR_VELOCITY >= 0)
Lightvalve 13:747daba9cf59 1377 VEL_POINT = VEL_POINT + 1;
Lightvalve 13:747daba9cf59 1378 else
Lightvalve 13:747daba9cf59 1379 VEL_POINT = VEL_POINT - 1;
Lightvalve 13:747daba9cf59 1380 }
Lightvalve 13:747daba9cf59 1381 else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1382 {
Lightvalve 13:747daba9cf59 1383 Ref_Valve_Pos_Old = Ref_Valve_Pos;
Lightvalve 13:747daba9cf59 1384 FINAL_POS = CUR_POSITION;
Lightvalve 13:747daba9cf59 1385 if(VEL_POINT >= 0)
Lightvalve 13:747daba9cf59 1386 DZ_DIRECTION = -1;
Lightvalve 13:747daba9cf59 1387 else
Lightvalve 13:747daba9cf59 1388 DZ_DIRECTION = 1;
Lightvalve 13:747daba9cf59 1389 if(abs(FINAL_POS - START_POS) < 100)
Lightvalve 13:747daba9cf59 1390 DZ_DIRECTION = 1;
Lightvalve 13:747daba9cf59 1391
Lightvalve 13:747daba9cf59 1392 VEL_POINT = 0;
Lightvalve 13:747daba9cf59 1393
Lightvalve 13:747daba9cf59 1394
Lightvalve 13:747daba9cf59 1395 VALVE_DZ_timer = 0;
Lightvalve 13:747daba9cf59 1396 //CAN_TX_PRES((int16_t) (3), (int16_t) (DZ_index));
Lightvalve 13:747daba9cf59 1397 DZ_index= DZ_index *2;
Lightvalve 13:747daba9cf59 1398 if(DZ_index >= 128)
Lightvalve 13:747daba9cf59 1399 {
Lightvalve 13:747daba9cf59 1400 SECOND_DZ = Ref_Valve_Pos;
Lightvalve 13:747daba9cf59 1401 DDV_CENTER = (int) (0.5 * (double) (FIRST_DZ) + 0.5 * (double) (SECOND_DZ));
Lightvalve 13:747daba9cf59 1402 first_check = 0;
Lightvalve 13:747daba9cf59 1403 VALVE_DEADZONE_MINUS = FIRST_DZ;
Lightvalve 13:747daba9cf59 1404 spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ);
Lightvalve 13:747daba9cf59 1405 VALVE_DEADZONE_PLUS = SECOND_DZ;
Lightvalve 13:747daba9cf59 1406 spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ);
Lightvalve 13:747daba9cf59 1407 spi_eeprom_write(RID_DDV_CNETER, DDV_CENTER);
Lightvalve 13:747daba9cf59 1408 CAN_TX_PRES((int16_t) FIRST_DZ, (int16_t) SECOND_DZ);
Lightvalve 13:747daba9cf59 1409 CONTROL_MODE = MODE_NO_ACT;
Lightvalve 13:747daba9cf59 1410 DZ_index = 1;
Lightvalve 13:747daba9cf59 1411 }
Lightvalve 13:747daba9cf59 1412
Lightvalve 13:747daba9cf59 1413 //CAN_TX_PRES((int16_t) (4), (int16_t) (DZ_index));
Lightvalve 13:747daba9cf59 1414 }
Lightvalve 13:747daba9cf59 1415 }
Lightvalve 13:747daba9cf59 1416 }
Lightvalve 13:747daba9cf59 1417 break;
Lightvalve 13:747daba9cf59 1418 }
Lightvalve 13:747daba9cf59 1419
Lightvalve 13:747daba9cf59 1420 case MODE_DDV_POS_VS_FLOWRATE:
Lightvalve 13:747daba9cf59 1421 {
Lightvalve 13:747daba9cf59 1422 VALVE_FR_timer = VALVE_FR_timer + 1;
Lightvalve 13:747daba9cf59 1423 if(first_check == 0)
Lightvalve 13:747daba9cf59 1424 {
Lightvalve 13:747daba9cf59 1425 if(VALVE_FR_timer < (int) (1.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1426 {
Lightvalve 13:747daba9cf59 1427 VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
Lightvalve 13:747daba9cf59 1428 CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6));
Lightvalve 13:747daba9cf59 1429 }
Lightvalve 13:747daba9cf59 1430 else if(VALVE_FR_timer == (int) (1.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1431 {
Lightvalve 13:747daba9cf59 1432 VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
Lightvalve 13:747daba9cf59 1433 pos_plus_end = CUR_POSITION;
Lightvalve 13:747daba9cf59 1434 // CAN_TX_PRES((int16_t) (VALVE_PWM_RAW), (int16_t) (7));
Lightvalve 13:747daba9cf59 1435 }
Lightvalve 13:747daba9cf59 1436 else if(VALVE_FR_timer < (int) (2.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1437 {
Lightvalve 13:747daba9cf59 1438 VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
Lightvalve 13:747daba9cf59 1439 }
Lightvalve 13:747daba9cf59 1440 else if(VALVE_FR_timer == (int) (2.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1441 {
Lightvalve 13:747daba9cf59 1442 // CAN_TX_PRES((int16_t) (VALVE_PWM_RAW), (int16_t) (8));
Lightvalve 13:747daba9cf59 1443 VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
Lightvalve 13:747daba9cf59 1444 pos_minus_end = CUR_POSITION;
Lightvalve 13:747daba9cf59 1445 first_check = 1;
Lightvalve 13:747daba9cf59 1446 VALVE_FR_timer = 0;
Lightvalve 13:747daba9cf59 1447 Ref_Valve_Pos = DDV_CENTER;
Lightvalve 13:747daba9cf59 1448 ID_index = 0;
Lightvalve 13:747daba9cf59 1449 max_check = 0;
Lightvalve 13:747daba9cf59 1450 min_check = 0;
Lightvalve 13:747daba9cf59 1451 }
Lightvalve 13:747daba9cf59 1452 }
Lightvalve 13:747daba9cf59 1453 else
Lightvalve 13:747daba9cf59 1454 {
Lightvalve 13:747daba9cf59 1455 if(VALVE_FR_timer < (int) (1.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1456 {
Lightvalve 13:747daba9cf59 1457 VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01* (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION);
Lightvalve 13:747daba9cf59 1458 }
Lightvalve 13:747daba9cf59 1459 else if(VALVE_FR_timer == (int) (1.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1460 {
Lightvalve 13:747daba9cf59 1461 data_num = 0;
Lightvalve 13:747daba9cf59 1462 Ref_Valve_Pos = 50*(ID_index_array[ID_index]) + DDV_CENTER;
Lightvalve 13:747daba9cf59 1463
Lightvalve 13:747daba9cf59 1464 VALVE_POS_CONTROL(Ref_Valve_Pos);
Lightvalve 13:747daba9cf59 1465 START_POS = CUR_POSITION;
Lightvalve 13:747daba9cf59 1466 }
Lightvalve 13:747daba9cf59 1467 else if(VALVE_FR_timer < (int) (5.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1468 {
Lightvalve 13:747daba9cf59 1469 Ref_Valve_Pos = 50*(ID_index_array[ID_index]) + DDV_CENTER;
Lightvalve 13:747daba9cf59 1470 VALVE_POS_CONTROL(Ref_Valve_Pos);
Lightvalve 13:747daba9cf59 1471 data_num = data_num + 1;
Lightvalve 13:747daba9cf59 1472 if(abs(0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION) > 20000)
Lightvalve 13:747daba9cf59 1473 {
Lightvalve 13:747daba9cf59 1474 FINAL_POS = CUR_POSITION;
Lightvalve 13:747daba9cf59 1475 one_period_end = 1;
Lightvalve 13:747daba9cf59 1476 }
Lightvalve 13:747daba9cf59 1477 }
Lightvalve 13:747daba9cf59 1478 else if(VALVE_FR_timer == (int) (5.0 * (double) TMR_FREQ_5k))
Lightvalve 13:747daba9cf59 1479 {
Lightvalve 13:747daba9cf59 1480 FINAL_POS = CUR_POSITION;
Lightvalve 13:747daba9cf59 1481 one_period_end = 1;
Lightvalve 13:747daba9cf59 1482 }
Lightvalve 13:747daba9cf59 1483
Lightvalve 13:747daba9cf59 1484 if(one_period_end == 1){
Lightvalve 13:747daba9cf59 1485 if(Ref_Valve_Pos > VALVE_MAX_POS)
Lightvalve 13:747daba9cf59 1486 {
Lightvalve 13:747daba9cf59 1487 max_check = 1;
Lightvalve 13:747daba9cf59 1488 }
Lightvalve 13:747daba9cf59 1489 else if(Ref_Valve_Pos < VALVE_MIN_POS)
Lightvalve 13:747daba9cf59 1490 {
Lightvalve 13:747daba9cf59 1491 min_check = 1;
Lightvalve 13:747daba9cf59 1492 }
Lightvalve 13:747daba9cf59 1493 JOINT_VEL[ID_index] = (FINAL_POS - START_POS) / data_num * TMR_FREQ_5k; // pulse/sec
Lightvalve 13:747daba9cf59 1494 spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0 + ID_index, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF));
Lightvalve 13:747daba9cf59 1495 spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0_1 + ID_index, (int16_t) ((JOINT_VEL[ID_index] >> 16) & 0xFFFF));
Lightvalve 13:747daba9cf59 1496 VALVE_FR_timer = 0;
Lightvalve 13:747daba9cf59 1497 one_period_end = 0;
Lightvalve 13:747daba9cf59 1498 ID_index= ID_index +1;
Lightvalve 13:747daba9cf59 1499 // CAN_TX_PRES((int16_t) (Ref_Valve_Pos), (int16_t) (ID_index));
Lightvalve 13:747daba9cf59 1500 }
Lightvalve 13:747daba9cf59 1501
Lightvalve 13:747daba9cf59 1502 if(max_check == 1 && min_check == 1)
Lightvalve 13:747daba9cf59 1503 {
Lightvalve 13:747daba9cf59 1504 spi_eeprom_write(RID_VALVE_POS_NUM, (int16_t) (ID_index));
Lightvalve 13:747daba9cf59 1505 VALVE_POS_NUM = ID_index;
Lightvalve 13:747daba9cf59 1506 ID_index = 0;
Lightvalve 13:747daba9cf59 1507 first_check = 0;
Lightvalve 13:747daba9cf59 1508 VALVE_FR_timer = 0;
Lightvalve 13:747daba9cf59 1509 // CAN_TX_PRES((int16_t) (123), (int16_t) (123));
Lightvalve 13:747daba9cf59 1510 CONTROL_MODE = MODE_NO_ACT;
Lightvalve 13:747daba9cf59 1511 }
Lightvalve 13:747daba9cf59 1512 }
Lightvalve 13:747daba9cf59 1513 break;
Lightvalve 13:747daba9cf59 1514 }
Lightvalve 12:6f2531038ea4 1515 default:
Lightvalve 12:6f2531038ea4 1516 break;
Lightvalve 12:6f2531038ea4 1517 }
Lightvalve 12:6f2531038ea4 1518
Lightvalve 13:747daba9cf59 1519 if (FLAG_VALVE_OUTPUT_CALIB) {
Lightvalve 13:747daba9cf59 1520 // Valve Dead Zone (Mechanical dead-zone canceling)
Lightvalve 13:747daba9cf59 1521 // if (CONTROL_MODE != MODE_VALVE_NULLING_AND_DEADZONE_SETTING) {
Lightvalve 13:747daba9cf59 1522 // if (VALVE_PWM_RAW > 0.) {
Lightvalve 13:747daba9cf59 1523 // VALVE_PWM_VALVE_DZ = (int) VALVE_PWM_RAW + VALVE_DEADZONE_PLUS;
Lightvalve 13:747daba9cf59 1524 // } else if (VALVE_PWM_RAW < 0.) {
Lightvalve 13:747daba9cf59 1525 // VALVE_PWM_VALVE_DZ = (int) VALVE_PWM_RAW + VALVE_DEADZONE_MINUS;
Lightvalve 13:747daba9cf59 1526 // } else VALVE_PWM_VALVE_DZ = (int) VALVE_PWM_RAW + VALVE_CENTER;
Lightvalve 13:747daba9cf59 1527 // } else VALVE_PWM_VALVE_DZ = (int) VALVE_PWM_RAW;
Lightvalve 13:747daba9cf59 1528
Lightvalve 13:747daba9cf59 1529 if(VALVE_PWM_RAW>0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS;
Lightvalve 13:747daba9cf59 1530 else if(VALVE_PWM_RAW<0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS;
Lightvalve 13:747daba9cf59 1531
Lightvalve 13:747daba9cf59 1532 VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + VALVE_CENTER;
Lightvalve 12:6f2531038ea4 1533
Lightvalve 13:747daba9cf59 1534 // Output Voltage Linearization and Dead Zone Cancellation (Electrical dead-zone)
Lightvalve 13:747daba9cf59 1535 double CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ/PWM_RESOL*SUPPLY_VOLTAGE*1000.0; // convert PWM duty to mV
Lightvalve 13:747daba9cf59 1536 double CUR_PWM_DZ = PWM_duty_byLT(CUR_PWM_nonlin);
Lightvalve 13:747daba9cf59 1537
Lightvalve 13:747daba9cf59 1538 if (CUR_PWM_DZ > 0) CUR_PWM = (int)CUR_PWM_DZ + 143;
Lightvalve 13:747daba9cf59 1539 else if (CUR_PWM_DZ < 0) CUR_PWM = (int)CUR_PWM_DZ - 138;
Lightvalve 13:747daba9cf59 1540 else CUR_PWM = CUR_PWM_DZ;
Lightvalve 13:747daba9cf59 1541 } else {
Lightvalve 13:747daba9cf59 1542 CUR_PWM = VALVE_PWM_RAW;
Lightvalve 13:747daba9cf59 1543 }
Lightvalve 13:747daba9cf59 1544
Lightvalve 13:747daba9cf59 1545 VALVE_PWM(CUR_PWM, VALVE_VOLTAGE_LIMIT, SUPPLY_VOLTAGE);
Lightvalve 12:6f2531038ea4 1546
Lightvalve 13:747daba9cf59 1547 //CAN ----------------------------------------------------------------------
Lightvalve 13:747daba9cf59 1548 if (TMR3_COUNT_CAN_TX % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
Lightvalve 13:747daba9cf59 1549
Lightvalve 13:747daba9cf59 1550 if (flag_data_request[0] == HIGH) {
Lightvalve 13:747daba9cf59 1551 //position+velocity
Lightvalve 13:747daba9cf59 1552 CAN_TX_POSITION((long) CUR_POSITION, (long) CUR_VELOCITY);
Lightvalve 13:747daba9cf59 1553 // CAN_TX_POSITION((int) (PRES_A_VREF * 100.), (int) (PRES_B_VREF * 100.));
Lightvalve 13:747daba9cf59 1554 // CAN_TX_POSITION((long) CUR_PRES_A_BAR, (long) CUR_PRES_B_BAR);
Lightvalve 13:747daba9cf59 1555 }
Lightvalve 13:747daba9cf59 1556
Lightvalve 13:747daba9cf59 1557 if (flag_data_request[1] == HIGH) {
Lightvalve 13:747daba9cf59 1558 //torque
Lightvalve 13:747daba9cf59 1559 //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM * 100.));
Lightvalve 13:747daba9cf59 1560 CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM));
Lightvalve 13:747daba9cf59 1561 // CAN_TX_TORQUE((int16_t) DZ_temp_cnt);
Lightvalve 13:747daba9cf59 1562 }
Lightvalve 13:747daba9cf59 1563
Lightvalve 13:747daba9cf59 1564 if (flag_data_request[2] == HIGH) {
Lightvalve 13:747daba9cf59 1565 //pressure A and B
Lightvalve 13:747daba9cf59 1566 CAN_TX_PRES((int16_t) (CUR_PRES_A), (int16_t) (CUR_PRES_B)); // CUR_PRES_X : 0(0bar)~4096(210bar)
Lightvalve 13:747daba9cf59 1567 // CAN_TX_PRES((int16_t) (CUR_PRES_A_BAR * 100.), (int16_t) (CUR_PRES_B_BAR * 100.));
Lightvalve 13:747daba9cf59 1568 // CAN_TX_PRES((int16_t) ((DEADZONE_MINUS + 1.)*1000.), (int16_t) ((DEADZONE_PLUS + 1.))*1000.);
Lightvalve 13:747daba9cf59 1569 // CAN_TX_PRES((int16_t) DZ_dir, (int16_t) ((VALVE_DEADZONE_PLUS + 1.))*1000.);
Lightvalve 13:747daba9cf59 1570
Lightvalve 13:747daba9cf59 1571 }
Lightvalve 13:747daba9cf59 1572
Lightvalve 13:747daba9cf59 1573 if (flag_data_request[3] == HIGH) {
Lightvalve 13:747daba9cf59 1574 //PWM
Lightvalve 13:747daba9cf59 1575 CAN_TX_PWM((int16_t) CUR_PWM);
Lightvalve 13:747daba9cf59 1576 // CAN_TX_PWM((int16_t) (CONTROL_MODE));
Lightvalve 13:747daba9cf59 1577 // CAN_TX_PWM((int16_t) cnt_vel_findhome);
Lightvalve 13:747daba9cf59 1578 // CAN_TX_PWM((int16_t) (VALVE_VOLTAGE * 1000.));
Lightvalve 13:747daba9cf59 1579 // CAN_TX_PWM((int16_t) (VALVE_VOLTAGE_VALVE_DZ * 1000.));
Lightvalve 13:747daba9cf59 1580
Lightvalve 13:747daba9cf59 1581 }
Lightvalve 13:747daba9cf59 1582
Lightvalve 13:747daba9cf59 1583 if (flag_data_request[4] == HIGH) {
Lightvalve 13:747daba9cf59 1584 //valve position
Lightvalve 13:747daba9cf59 1585 CAN_TX_VALVE_POSITION((int16_t) (I2C_val));
Lightvalve 13:747daba9cf59 1586 //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF));
Lightvalve 13:747daba9cf59 1587 //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0_1, (int16_t) ((0xFEF1>>16) & 0xFFFF));
Lightvalve 13:747daba9cf59 1588
Lightvalve 13:747daba9cf59 1589
Lightvalve 13:747daba9cf59 1590 //CAN_TX_VALVE_POSITION((int16_t) (VALVE_POS_NUM));
Lightvalve 13:747daba9cf59 1591 // CAN_TX_VALVE_POSITIOfxN((int16_t) (VALVE_FF_VOLTAGE * PWM_RESOL / SUPPLY_VOLTAGE));
Lightvalve 13:747daba9cf59 1592 // CAN_TX_VALVE_POSITION((int16_t) P_GAIN_JOINT_POSITION);
Lightvalve 13:747daba9cf59 1593 // CAN_TX_VALVE_POSITION((int16_t) Ref_Joint_Pos);
Lightvalve 13:747daba9cf59 1594 // CAN_TX_VALVE_POSITION((int16_t) flag_flowrate);
Lightvalve 13:747daba9cf59 1595 }
Lightvalve 13:747daba9cf59 1596
Lightvalve 13:747daba9cf59 1597 TMR3_COUNT_CAN_TX = 0;
Lightvalve 13:747daba9cf59 1598
Lightvalve 13:747daba9cf59 1599 }
Lightvalve 13:747daba9cf59 1600 TMR3_COUNT_CAN_TX++;
Lightvalve 13:747daba9cf59 1601
jobuuu 1:e04e563be5ce 1602 /*******************************************************
Lightvalve 13:747daba9cf59 1603 *** Valve Control
jobuuu 1:e04e563be5ce 1604 ********************************************************/
jobuuu 7:e9086c72bb22 1605 ValveControl(CONTROL_MODE);
Lightvalve 13:747daba9cf59 1606
jobuuu 7:e9086c72bb22 1607 double t = (double)CNT_TMR4*DT_TMR4;
Lightvalve 13:747daba9cf59 1608 double T = 5.0;
jobuuu 7:e9086c72bb22 1609 V_out = 1000.0*sin(2.0*PI*t/T); // V_out : -5000.0mV~5000.0mV(full duty)
jobuuu 7:e9086c72bb22 1610 // if(V_out > 0.0) V_out = 1000.0;
Lightvalve 13:747daba9cf59 1611 // else if(V_out < 0.0) V_out = -1000.0;
jobuuu 5:a4319f79457b 1612
jobuuu 7:e9086c72bb22 1613 /*******************************************************
jobuuu 7:e9086c72bb22 1614 *** PWM
jobuuu 7:e9086c72bb22 1615 ********************************************************/
jobuuu 5:a4319f79457b 1616 PWM_out= V_out/12000.0; // Full duty : 12000.0mV
Lightvalve 13:747daba9cf59 1617
jobuuu 2:a1c0a37df760 1618 // Saturation of output voltage to 5.0V
jobuuu 5:a4319f79457b 1619 if(PWM_out > 0.41667) PWM_out=0.41667; //5.0/12.0 = 0.41667
jobuuu 5:a4319f79457b 1620 else if (PWM_out < -0.41667) PWM_out=-0.41667;
Lightvalve 13:747daba9cf59 1621
jobuuu 2:a1c0a37df760 1622 if (PWM_out>0.0) {
jobuuu 2:a1c0a37df760 1623 dtc_v=0.0;
jobuuu 1:e04e563be5ce 1624 dtc_w=PWM_out;
jobuuu 1:e04e563be5ce 1625 } else {
jobuuu 2:a1c0a37df760 1626 dtc_v=-PWM_out;
jobuuu 2:a1c0a37df760 1627 dtc_w=0.0;
jobuuu 1:e04e563be5ce 1628 }
Lightvalve 13:747daba9cf59 1629
jobuuu 1:e04e563be5ce 1630 //pwm
jobuuu 2:a1c0a37df760 1631 TIM4->CCR2 = (PWM_ARR)*(1.0-dtc_v);
jobuuu 2:a1c0a37df760 1632 TIM4->CCR1 = (PWM_ARR)*(1.0-dtc_w);
Lightvalve 13:747daba9cf59 1633
jobuuu 1:e04e563be5ce 1634 /*******************************************************
jobuuu 2:a1c0a37df760 1635 *** Data Send (CAN) & Print out (UART)
jobuuu 1:e04e563be5ce 1636 ********************************************************/
Lightvalve 11:82d8768d7351 1637 //if((CNT_TMR3%40)==0){
Lightvalve 11:82d8768d7351 1638 // msg.id = 50;
Lightvalve 11:82d8768d7351 1639 // msg.len = 4;
Lightvalve 11:82d8768d7351 1640 // int temp_CUR = (int)(cur.sen*1000.0);
Lightvalve 11:82d8768d7351 1641 // msg.data[0]=0x00FF&temp_CUR;
Lightvalve 11:82d8768d7351 1642 // msg.data[1]=0x00FF&(temp_CUR>>8);
Lightvalve 11:82d8768d7351 1643 // int temp_PWM = (int)(V_out);
Lightvalve 11:82d8768d7351 1644 // msg.data[2]=0x00FF&temp_PWM;
Lightvalve 11:82d8768d7351 1645 // msg.data[3]=0x00FF&(temp_PWM>>8);
Lightvalve 11:82d8768d7351 1646 // can.write(msg);
Lightvalve 11:82d8768d7351 1647 // }
Lightvalve 11:82d8768d7351 1648
Lightvalve 13:747daba9cf59 1649 if((CNT_TMR3%5000)==0) {
Lightvalve 13:747daba9cf59 1650 if(LED==1) {
Lightvalve 11:82d8768d7351 1651 LED=0;
Lightvalve 13:747daba9cf59 1652 } else
Lightvalve 11:82d8768d7351 1653 LED = 1;
Lightvalve 11:82d8768d7351 1654 // LED != LED;
jobuuu 2:a1c0a37df760 1655 }
jobuuu 1:e04e563be5ce 1656
jobuuu 1:e04e563be5ce 1657 /*******************************************************
jobuuu 1:e04e563be5ce 1658 *** Timer Counting & etc.
jobuuu 1:e04e563be5ce 1659 ********************************************************/
Lightvalve 13:747daba9cf59 1660 CNT_TMR3++;
jobuuu 1:e04e563be5ce 1661 }
Lightvalve 11:82d8768d7351 1662 TIM3->SR = 0x0; // reset the status register
jobuuu 7:e9086c72bb22 1663 }
jobuuu 7:e9086c72bb22 1664
jobuuu 7:e9086c72bb22 1665 void ValveControl(unsigned int ControlMode)
jobuuu 7:e9086c72bb22 1666 {
jobuuu 7:e9086c72bb22 1667 switch (ControlMode) {
jobuuu 7:e9086c72bb22 1668 case MODE_NO_ACT: // 0
jobuuu 7:e9086c72bb22 1669 V_out = 0.0;
jobuuu 7:e9086c72bb22 1670 break;
jobuuu 7:e9086c72bb22 1671 case MODE_VALVE_OPEN_LOOP: // 1
jobuuu 7:e9086c72bb22 1672 V_out = Vout.ref;
Lightvalve 13:747daba9cf59 1673 break;
jobuuu 7:e9086c72bb22 1674 case MODE_VALVE_POSITION_CONTROL: // 2
jobuuu 7:e9086c72bb22 1675 CurrentControl();
Lightvalve 13:747daba9cf59 1676 break;
jobuuu 7:e9086c72bb22 1677 case MODE_JOINT_POSITION_TORQUE_CONTROL_PWM: // 3
jobuuu 7:e9086c72bb22 1678 V_out = 0.0;
Lightvalve 13:747daba9cf59 1679 break;
jobuuu 7:e9086c72bb22 1680 case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION: // 4
jobuuu 7:e9086c72bb22 1681 double I_REF_POS_FB = 0.0; // I_REF by Position Feedback
jobuuu 7:e9086c72bb22 1682 double I_REF_POS_FF = 0.0; // I_REF by Position Feedforward
Lightvalve 13:747daba9cf59 1683
jobuuu 7:e9086c72bb22 1684 // feedback input for position control
jobuuu 7:e9086c72bb22 1685 pos.err = pos.ref - pos.sen;
jobuuu 7:e9086c72bb22 1686 double alpha_update_vel = 1.0/(1.0+(double)FREQ_TMR4/(2.0*3.1415*50.0)); // f_cutoff : 50Hz
jobuuu 7:e9086c72bb22 1687 double err_diff = (pos.err - pos.err_old)*(double)FREQ_5k;
jobuuu 7:e9086c72bb22 1688 pos.err_diff = (1.0-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff;
jobuuu 7:e9086c72bb22 1689 pos.err_old = pos.err;
jobuuu 7:e9086c72bb22 1690 I_REF_POS_FB = 0.001*((double)P_GAIN_JOINT_POSITION * pos.err + (double)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1);
Lightvalve 13:747daba9cf59 1691
jobuuu 7:e9086c72bb22 1692 // feedforward input for position control
Lightvalve 13:747daba9cf59 1693 double Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
jobuuu 7:e9086c72bb22 1694 double K_ff = 1.3;
jobuuu 7:e9086c72bb22 1695 double K_v = 0.0;
jobuuu 7:e9086c72bb22 1696 if(Vel_Act_Ref > 0) K_v = 1.0/100.0; // open, tuning. (deg/s >> mA)
Lightvalve 13:747daba9cf59 1697 if(Vel_Act_Ref < 0) K_v = 1.0/100.0; // close, tuning. (deg/s >> mA)
jobuuu 7:e9086c72bb22 1698 I_REF_POS_FF = K_ff*K_v*Vel_Act_Ref;
Lightvalve 13:747daba9cf59 1699
jobuuu 7:e9086c72bb22 1700 cur.ref = I_REF_POS_FF + I_REF_POS_FB;
Lightvalve 13:747daba9cf59 1701 break;
jobuuu 7:e9086c72bb22 1702 case MODE_TEST_CURRENT_CONTROL: // 9
jobuuu 7:e9086c72bb22 1703 V_out = 0.0;
Lightvalve 13:747daba9cf59 1704 break;
jobuuu 7:e9086c72bb22 1705 case MODE_TEST_PWM_CONTROL: // 10
jobuuu 7:e9086c72bb22 1706 V_out = 0.0;
Lightvalve 13:747daba9cf59 1707 break;
jobuuu 7:e9086c72bb22 1708 case MODE_FIND_HOME: // 22
jobuuu 7:e9086c72bb22 1709 V_out = 0.0;
Lightvalve 13:747daba9cf59 1710 break;
jobuuu 7:e9086c72bb22 1711 default:
jobuuu 7:e9086c72bb22 1712 V_out = 0.0;
jobuuu 7:e9086c72bb22 1713 break;
Lightvalve 13:747daba9cf59 1714
jobuuu 7:e9086c72bb22 1715 }
jobuuu 7:e9086c72bb22 1716 }
jobuuu 7:e9086c72bb22 1717
Lightvalve 13:747daba9cf59 1718 void CurrentControl()
Lightvalve 13:747daba9cf59 1719 {
jobuuu 7:e9086c72bb22 1720 cur.err = cur.ref - cur.sen;
jobuuu 7:e9086c72bb22 1721 cur.err_int = cur.err_int + cur.err*DT_TMR4;
jobuuu 7:e9086c72bb22 1722 cur.err_diff = (cur.err - cur.err_old)*FREQ_TMR4;
jobuuu 7:e9086c72bb22 1723 cur.err_old = cur.err;
jobuuu 7:e9086c72bb22 1724
jobuuu 7:e9086c72bb22 1725 double R_model = 150.0; // ohm
jobuuu 7:e9086c72bb22 1726 double L_model = 0.3;
jobuuu 7:e9086c72bb22 1727 double w0 = 2.0*3.14*90.0;
jobuuu 7:e9086c72bb22 1728 double KP_I = L_model*w0;
jobuuu 7:e9086c72bb22 1729 double KI_I = R_model*w0;
jobuuu 7:e9086c72bb22 1730 double KD_I = 0.0;
jobuuu 7:e9086c72bb22 1731
jobuuu 7:e9086c72bb22 1732 double FF_gain = 0.0;
jobuuu 7:e9086c72bb22 1733 V_out = (int) (KP_I * cur.err + KI_I * cur.err_int + KD_I * cur.err_diff);
jobuuu 7:e9086c72bb22 1734 // V_out = V_out + FF_gain * (R_model*I_REF); // Unit : mV
jobuuu 7:e9086c72bb22 1735 V_out = V_out + FF_gain * (R_model*cur.ref + L_model*cur.ref_diff); // Unit : mV
Lightvalve 13:747daba9cf59 1736
Lightvalve 13:747daba9cf59 1737 double Ka = 5.0/KP_I;
jobuuu 7:e9086c72bb22 1738 if(V_out > V_MAX) {
jobuuu 7:e9086c72bb22 1739 V_rem = V_out-V_MAX;
jobuuu 7:e9086c72bb22 1740 V_rem = Ka*V_rem;
jobuuu 7:e9086c72bb22 1741 V_out = V_MAX;
jobuuu 7:e9086c72bb22 1742 cur.err_int = cur.err_int - V_rem*DT_5k;
jobuuu 7:e9086c72bb22 1743 } else if(V_out < -V_MAX) {
jobuuu 7:e9086c72bb22 1744 V_rem = V_out-(-V_MAX);
Lightvalve 13:747daba9cf59 1745 V_rem = Ka*V_rem;
jobuuu 7:e9086c72bb22 1746 V_out = -V_MAX;
jobuuu 7:e9086c72bb22 1747 cur.err_int = cur.err_int - V_rem*DT_5k;
Lightvalve 13:747daba9cf59 1748 }
GiJeongKim 0:51c43836c1d7 1749 }