Kim GiJeong
/
HydraulicControlBoard_LIGHT_GJ
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 13:747daba9cf59, committed 2019-08-27
- Comitter:
- Lightvalve
- Date:
- Tue Aug 27 06:50:54 2019 +0000
- Parent:
- 12:6f2531038ea4
- Child:
- 14:8e7590227d22
- Commit message:
- 1908271550
Changed in this revision
--- a/function_utilities/function_utilities.cpp Tue Aug 27 05:32:44 2019 +0000
+++ b/function_utilities/function_utilities.cpp Tue Aug 27 06:50:54 2019 +0000
@@ -177,8 +177,100 @@
JUMP_LANDING, //4
};
+double CUR_PRES_DIFF_BAR = 0.0;
+double CUR_PRES_A_sum = 0.0;
+double CUR_PRES_B_sum = 0.0;
+double CUR_PRES_A_mean = 0.0;
+double CUR_PRES_B_mean = 0.0;
+double CUR_TORQUE_sum = 0.0;
+double CUR_TORQUE_mean = 0.0;
+double PRES_A_NULL = 200.0;
+double PRES_B_NULL = 200.0;
+double TORQUE_NULL = 3900;
+double Ref_PWM = 0.0;
+double Ref_Valve_Pos = 0.0;
+double Ref_Joint_Pos = 0.0;
+double Ref_Joint_Vel = 0.0;
+double Ref_Joint_Torq = 0.0;
+double Ref_Valve_Pos_Old = 0.0;
+int VALVE_ID_timer = 0;
+int VALVE_DZ_timer = 0;
+int VALVE_FR_timer = 0;
+int VALVE_POS_TMP = 0;
+int JOINT_VEL_TMP = 0;
+int DDV_POS_AVG = 0;
+int VALVE_POS_AVG[100] = {0};
+int VALVE_POS_AVG_OLD = 0;
+int data_num = 0;
+int ID_index = 0;
+int DZ_index = 1;
+int ID_index_array[100] = {0};
+int FR_index_array[100] = {0};
+int first_check = 0;
+double init_time = 0.0;
+int VEL_POINT = 0;
+int DZ_case = 0;
+int START_POS = 0;
+int FINAL_POS = 0;
+int DZ_DIRECTION = 0;
+int FIRST_DZ = 0;
+int SECOND_DZ = 0;
+int DZ_NUM = 0;
+int one_period_end = 0;
+double Ref_Vel_Test = 0.0;
+long TMR2_FOR_SLOW_LOGGING = 0;
+int velcount = 0;
+char max_check = 0;
+char min_check = 0;
+
+double valve_pos_err = 0.0, valve_pos_err_old = 0.0, valve_pos_err_diff = 0.0, valve_pos_err_sum = 0.0;
+double joint_pos_err = 0.0, joint_pos_err_old = 0.0, joint_pos_err_diff = 0.0, joint_pos_err_sum = 0.0;
+double joint_torq_err = 0.0, joint_torq_err_old = 0.0, joint_torq_err_diff = 0.0, joint_torq_err_sum = 0.0;
+double VALVE_PWM_RAW_POS = 0.0, VALVE_PWM_RAW_TORQ = 0.0;
+
+double CUR_FLOWRATE = 0.0;
+double VALVE_FF_VOLTAGE = 0.0;
+
+int pos_plus_end = 0;
+int pos_minus_end = 0;
+
+bool need_enc_init = false;
+
+int temp_time = 0;
+
+double CUR_VELOCITY_sum = 0.0;
+double temp_vel_sum = 0.0;
+
+int DZ_dir = 0;
+int DZ_temp_cnt = 0;
+int DZ_temp_cnt2 = 0;
+int DZ_end = 2;
+int flag_flowrate = 0;
+int fl_temp_cnt = 0;
+int fl_temp_cnt2 = 0;
+int cur_vel_sum = 0;
+
+// find home
+int CUR_VELOCITY_OLD = 0;
+int cnt_findhome = 0;
+int cnt_vel_findhome = 0;
+int FINDHOME_VELOCITY = 0;
+int FINDHOME_VELOCITY_OLD = 0;
+int FINDHOME_POSITION = 0;
+int FINDHOME_POSITION_OLD = 0;
+
+// valve gain
+int check_vel_pos_init = 0;
+int check_vel_pos_fin = 0;
+int check_vel_pos_interv = 0;
+int valve_gain_repeat_cnt = 0;
+double VALVE_VOLTAGE = 0.0;
+
+double freq_fric_tune = 1.0;
+
+bool FLAG_VALVE_OUTPUT_CALIB = false;
/*******************************************************************************
* General math functions
@@ -446,11 +538,10 @@
}
-void ENC_SET(int32_t value) {
-
+void ENC_SET(int32_t value_e) {
spi_enc_set_clear();
- enc_offset = value;
- CUR_POSITION = value;
- ENC_pos_old = ENC_pos_cur = value;
+ enc_offset = value_e;
+ CUR_POSITION = value_e;
+ ENC_pos_old = ENC_pos_cur = value_e;
}
--- a/function_utilities/function_utilities.h Tue Aug 27 05:32:44 2019 +0000 +++ b/function_utilities/function_utilities.h Tue Aug 27 06:50:54 2019 +0000 @@ -11,7 +11,7 @@ void ENC_UPDATE(void); void ENC_SET_ZERO(void); -void ENC_SET(long value); +void ENC_SET(int32_t value); void VALVE_POSITION_INIT(void); void VALVE_PWM(int pwm, double vol_max, double vol_in);
--- a/main.cpp Tue Aug 27 05:32:44 2019 +0000
+++ b/main.cpp Tue Aug 27 06:50:54 2019 +0000
@@ -55,19 +55,8 @@
double V_MAX = 12000.0; // Maximum Voltage : 12V = 12000mV
double PWM_out=0.0;
-int ID_index_array[100] = {0};
-
-double CUR_PRES_DIFF_BAR = 0;
-double CUR_PRES_A_sum = 0.0;
-double CUR_PRES_B_sum = 0.0;
-double CUR_PRES_A_mean = 0.0;
-double CUR_PRES_B_mean = 0.0;
-double CUR_TORQUE_sum = 0.0;
-double CUR_TORQUE_mean = 0.0;
-double PRES_A_NULL = 200.0;
-double PRES_B_NULL = 200.0;
-double TORQUE_NULL = 3900;
+
// =============================================================================
// =============================================================================
// =============================================================================
@@ -75,8 +64,8 @@
/*******************************************************************************
* REFERENCE MODE
******************************************************************************/
-enum _REFERENCE_MODE{
- MODE_REF_NO_ACT = 0, //0
+enum _REFERENCE_MODE {
+ MODE_REF_NO_ACT = 0, //0
MODE_REF_DIRECT, //1
MODE_REF_COS_INC, //2
MODE_REF_LINE_INC, //3
@@ -87,23 +76,23 @@
/*******************************************************************************
* CONTROL MODE
******************************************************************************/
-enum _CONTROL_MODE{
+enum _CONTROL_MODE {
//control mode
MODE_NO_ACT = 0, //0
MODE_VALVE_OPEN_LOOP, //1
MODE_VALVE_POSITION_CONTROL, //2
-
+
MODE_JOINT_POSITION_TORQUE_CONTROL_PWM, //3
MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION, //4
MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING, //5
-
+
MODE_JOINT_POSITION_PRES_CONTROL_PWM, //6
MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION, //7
MODE_VALVE_POSITION_PRES_CONTROL_LEARNING, //8
-
+
MODE_TEST_CURRENT_CONTROL, //9
MODE_TEST_PWM_CONTROL, //10
-
+
//utility
MODE_TORQUE_SENSOR_NULLING = 20, //20
MODE_VALVE_NULLING_AND_DEADZONE_SETTING, //21
@@ -112,7 +101,7 @@
MODE_PRESSURE_SENSOR_NULLING, //24
MODE_PRESSURE_SENSOR_CALIB, //25
MODE_ROTARY_FRICTION_TUNING, //26
-
+
MODE_DDV_POS_VS_PWM_ID = 30, //30
MODE_DDV_DEADZONE_AND_CENTER, //31
MODE_DDV_POS_VS_FLOWRATE, //32
@@ -123,7 +112,7 @@
/*********************************
*** Initialization
*********************************/
-
+
LED = 1;
pc.baud(9600);
@@ -140,7 +129,7 @@
enc.format(8,0);
enc.frequency(5000000); //5M
make_delay();
-
+
// ADC init
Init_ADC();
make_delay();
@@ -149,7 +138,7 @@
Init_PWM();
TIM4->CR1 ^= TIM_CR1_UDIS;
make_delay();
-
+
// TMR3 init
Init_TMR3();
TIM3->CR1 ^= TIM_CR1_UDIS;
@@ -159,43 +148,134 @@
can.attach(&CAN_RX_HANDLER);
CAN_ID_INIT();
make_delay();
-
+
// spi _ enc
spi_enc_set_init();
make_delay();
-
+
//eeprom
ROM_INIT_DATA();
make_delay();
-
+
//DAC init
dac_1 = PRES_A_VREF/3.3;
dac_2 = PRES_B_VREF/3.3;
make_delay();
-
- for (int i=0; i<100; i++){
+
+ for (int i=0; i<100; i++) {
if(i%2==0)
ID_index_array[i] = - i * 0.5;
else
ID_index_array[i] = (i+1) * 0.5;
}
-
-
+
+
/************************************
*** Program is operating!
*************************************/
while(1) {
-
+
//spi _ enc
//int a = spi_enc_read();
-
+
//i2c
read_field(i2c_slave_addr1);
}
}
+int DDV_JOINT_POS_FF(double REF_JOINT_VEL){
+
+ int i = 0;
+ int Ref_Valve_Pos_FF = DDV_CENTER;
+ for(i=0; i<VALVE_POS_NUM; i++)
+ {
+ if(REF_JOINT_VEL >= min(JOINT_VEL[i],JOINT_VEL[i+1]) && REF_JOINT_VEL <= max(JOINT_VEL[i],JOINT_VEL[i+1]))
+ {
+ if(i==0)
+ {
+ Ref_Valve_Pos_FF = ((int) 50.0/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER;
+ }
+ else
+ {
+ 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];
+ }
+ break;
+ }
+ }
+ if(REF_JOINT_VEL > max(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2]))
+ {
+ Ref_Valve_Pos_FF = VALVE_MAX_POS;
+ }
+ else if(REF_JOINT_VEL < min(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2]))
+ {
+ Ref_Valve_Pos_FF = VALVE_MIN_POS;
+ }
+ return Ref_Valve_Pos_FF;
+}
+void VALVE_POS_CONTROL(double REF_VALVE_POS){
+ int i = 0;
+
+ valve_pos_err = REF_VALVE_POS - value;
+ valve_pos_err_diff = valve_pos_err - valve_pos_err_old;
+ valve_pos_err_old = valve_pos_err;
+ valve_pos_err_sum += valve_pos_err;
+ if (valve_pos_err_sum > 1000) valve_pos_err_sum = 1000;
+ if (valve_pos_err_sum<-1000) valve_pos_err_sum = -1000;
+
+ 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;
+
+ if(REF_VALVE_POS > VALVE_MAX_POS)
+ {
+ REF_VALVE_POS = VALVE_MAX_POS;
+ }
+ else if(REF_VALVE_POS < VALVE_MIN_POS)
+ {
+ REF_VALVE_POS = VALVE_MIN_POS;
+ }
+
+ for(i=0; i<16; i++)
+ {
+ 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]))
+ {
+ if(i==0)
+ {
+ 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]);
+ }
+ else
+ {
+ 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];
+ }
+ break;
+ }
+ }
+ VALVE_PWM_RAW = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
+}
+
+
+double PWM_duty_byLT(double Ref_V) {
+ double PWM_duty = 0.0;
+ if(Ref_V<LT_Voltage_Output[0]) {
+ PWM_duty = (Ref_V-LT_Voltage_Output[0])/1.5+LT_PWM_duty[0];
+ } else if (Ref_V>=LT_Voltage_Output[LT_MAX_IDX-1]) {
+ PWM_duty = (Ref_V-LT_Voltage_Output[LT_MAX_IDX-1])/1.5+LT_PWM_duty[LT_MAX_IDX-1];
+ } else {
+ int idx = 0;
+ for(idx=0; idx<LT_MAX_IDX-1; idx++) {
+ double ini_x = LT_Voltage_Output[idx];
+ double fin_x = LT_Voltage_Output[idx+1];
+ double ini_y = LT_PWM_duty[idx];
+ double fin_y = LT_PWM_duty[idx+1];
+ if(Ref_V>=ini_x && Ref_V<fin_x) {
+ PWM_duty = (fin_y-ini_y)/(fin_x-ini_x)*(Ref_V-ini_x) + ini_y;
+ break;
+ }
+ }
+ }
+
+ return PWM_duty;
+}
/*******************************************************************************
TIMER INTERRUPT
@@ -208,18 +288,18 @@
{
if ( TIM4->SR & TIM_SR_UIF ) {
/*******************************************************
- *** Sensor Read & Data Handling
+ *** Sensor Read & Data Handling
********************************************************/
-
- if((CNT_TMR4%2)==0){
-
+
+ if((CNT_TMR4%2)==0) {
+
//ADC
ADC3->CR2 |= 0x40000000; // adc _ 12bit
// a1=ADC1->DR;
// a1=ADC2->DR;
// int raw_cur = ADC3->DR;
while((ADC3->SR & 0b10));
-
+
double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz
double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA
cur.sen=cur.sen*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur);
@@ -229,12 +309,12 @@
// dac_1 = ADC1->DR;
// dac_2 = ADC2->DR;
-
-
+
+
/*******************************************************
*** Timer Counting & etc.
********************************************************/
- CNT_TMR4++;
+ CNT_TMR4++;
}
TIM4->SR = 0x0; // reset the status register
}
@@ -245,23 +325,21 @@
extern "C" void TIM3_IRQHandler(void)
{
if ( TIM3->SR & TIM_SR_UIF ) {
-
+
ENC_UPDATE();
CUR_PRES_A_BAR = (CUR_PRES_A - PRES_A_NULL) / PRES_SENSOR_A_PULSE_PER_BAR + 1.;
CUR_PRES_B_BAR = (CUR_PRES_B - PRES_B_NULL) / PRES_SENSOR_B_PULSE_PER_BAR + 1.;
//CUR_TORQUE_NM = (CUR_TORQUE - TORQUE_NULL) / (double) TORQUE_SENSOR_PULSE_PER_TORQUE; //
CUR_TORQUE_NM = CUR_TORQUE; //
CUR_TORQUE_NM_PRESS = 1. * (CUR_PRES_A_BAR - CUR_PRES_B_BAR);
-
- // Reference Loop
+
+ // Reference Loop
switch (REFERENCE_MODE) {
- case MODE_REF_NO_ACT:
- {
+ case MODE_REF_NO_ACT: {
break;
}
-
- case MODE_REF_DIRECT:
- {
+
+ case MODE_REF_DIRECT: {
if (FLAG_REFERENCE_VALVE_PWM) {
Ref_PWM = (double) REF_PWM;
}
@@ -277,9 +355,8 @@
}
break;
}
-
- case MODE_REF_COS_INC:
- {
+
+ case MODE_REF_COS_INC: {
if (FLAG_REFERENCE_VALVE_PWM) {
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;
}
@@ -300,10 +377,9 @@
}
break;
}
-
-
- case MODE_REF_LINE_INC:
- {
+
+
+ case MODE_REF_LINE_INC: {
if (FLAG_REFERENCE_VALVE_PWM) {
Ref_PWM = ((double) REF_PWM - (double) INIT_REF_PWM)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_PWM;
}
@@ -327,9 +403,8 @@
}
break;
}
-
- case MODE_REF_SIN_WAVE:
- {
+
+ case MODE_REF_SIN_WAVE: {
if (FLAG_REFERENCE_VALVE_PWM) {
Ref_PWM = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_PWM;
}
@@ -349,9 +424,8 @@
}
break;
}
-
- case MODE_REF_SQUARE_WAVE:
- {
+
+ case MODE_REF_SQUARE_WAVE: {
if (FLAG_REFERENCE_VALVE_PWM) {
Ref_PWM = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_PWM;
if (Ref_PWM >= REF_PWM) Ref_PWM = REF_MAG + REF_PWM;
@@ -379,33 +453,1172 @@
}
break;
}
+
+ default:
+ break;
+ }
+
+ // CONTROL LOOP ------------------------------------------------------------
+ switch (CONTROL_MODE) {
+ case MODE_NO_ACT:
+ {
+ // SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0);
+ VALVE_PWM_RAW = 0;
+ break;
+ }
+ case MODE_VALVE_OPEN_LOOP:
+ {
+ VALVE_PWM_RAW = Ref_PWM;
+ break;
+ }
+
+ case MODE_VALVE_POSITION_CONTROL:
+ {
+ VALVE_POS_CONTROL(Ref_Valve_Pos);
+ break;
+ }
+
+ case MODE_JOINT_POSITION_TORQUE_CONTROL_PWM:
+ {
+ double PWM_RAW_POS_FB = 0.0; // PWM by Position Feedback
+ double PWM_RAW_POS_FF = 0.0; // PWM by Position Feedforward
+ double PWM_RAW_FORCE_FB = 0.0; // PWM by Force Feedback
+
+ // feedback input for position control
+ joint_pos_err = Ref_Joint_Pos - (double) CUR_POSITION;
+ joint_pos_err_diff = joint_pos_err - joint_pos_err_old;
+ joint_pos_err_old = joint_pos_err;
+ joint_pos_err_sum += joint_pos_err;
+ if (joint_pos_err_sum > 1000) joint_pos_err_sum = 1000;
+ if (joint_pos_err_sum<-1000) joint_pos_err_sum = -1000;
+ // 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;
+ PWM_RAW_POS_FB = (double) P_GAIN_JOINT_POSITION * joint_pos_err;
+ PWM_RAW_POS_FB = PWM_RAW_POS_FB * 0.01;
+
+ // feedforward input for position control
+ double Ref_Joint_Vel_Act = Ref_Joint_Vel/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
+ double K_ff = 0.9;
+ if(Ref_Joint_Vel_Act > 0) K_ff = 0.90; // open
+ if(Ref_Joint_Vel_Act > 0) K_ff = 0.75; // close
+ PWM_RAW_POS_FF = K_ff*Ref_Joint_Vel_Act/0.50;
+
+ // torque feedback
+ // joint_torq_err = Ref_Joint_Torq - CUR_TORQUE_NM;
+ // joint_torq_err_diff = joint_torq_err - joint_torq_err_old;
+ // joint_torq_err_old = joint_torq_err;
+ // joint_torq_err_sum += joint_torq_err;
+ // if (joint_torq_err_sum > 1000) joint_torq_err_sum = 1000;
+ // if (joint_torq_err_sum<-1000) joint_torq_err_sum = -1000;
+ // 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;
+ // VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01;
+ PWM_RAW_FORCE_FB = 0.0;
+
+ // VALVE_PWM_RAW = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_POS + (double) COMPLIANCE_GAIN * 0.01 * VALVE_PWM_RAW_TORQ;
+ VALVE_PWM_RAW = PWM_RAW_POS_FF + PWM_RAW_POS_FB + PWM_RAW_FORCE_FB;
+
+ break;
+ }
+
+ case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION:
+ {
+ TMR2_COUNT_JOINT++;
+ double VALVE_POS_RAW_POS_FB = 0.0; // Valve Position by Position Feedback
+ double VALVE_POS_RAW_POS_FF = 0.0; // Valve Position by Position Feedforward
+ double VALVE_POS_RAW_FORCE_FB = 0.0; // Valve Position by Force Feedback
+ int DDV_JOINT_CAN = 0;
+ // feedback input for position control
+ joint_pos_err = Ref_Joint_Pos - (double) CUR_POSITION;
+ joint_pos_err_diff = joint_pos_err - joint_pos_err_old;
+ joint_pos_err_old = joint_pos_err;
+ joint_pos_err_sum += joint_pos_err;
+ if (joint_pos_err_sum > 1000) joint_pos_err_sum = 1000;
+ if (joint_pos_err_sum<-1000) joint_pos_err_sum = -1000;
+ 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;
+ VALVE_POS_RAW_POS_FB = VALVE_POS_RAW_POS_FB * 0.01;
+
+
+ //Ref_Joint_Vel = Ref_Vel_Test;
+ // feedforward input for position control
+ // double Ref_Joint_Vel_Act = Ref_Joint_Vel/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
+ // double K_ff = 0.9;
+ // if(Ref_Joint_Vel_Act > 0) K_ff = 0.90; // open
+ // if(Ref_Joint_Vel_Act > 0) K_ff = 0.75; // close
+ // VALVE_POS_RAW_POS_FF = K_ff*Ref_Joint_Vel_Act/0.50;
+
+ // torque feedback
+ // joint_torq_err = Ref_Joint_Torq - CUR_TORQUE_NM;
+ // joint_torq_err_diff = joint_torq_err - joint_torq_err_old;
+ // joint_torq_err_old = joint_torq_err;
+ // joint_torq_err_sum += joint_torq_err;
+ // if (joint_torq_err_sum > 1000) joint_torq_err_sum = 1000;
+ // if (joint_torq_err_sum<-1000) joint_torq_err_sum = -1000;
+ // 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;
+ // VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01;
+ VALVE_POS_RAW_FORCE_FB = 0.0;
+
+ Ref_Valve_Pos = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(Ref_Joint_Vel) + VALVE_POS_RAW_FORCE_FB;
+ //Ref_Valve_Pos = DDV_JOINT_POS_FF(Ref_Joint_Vel);
+
+ VALVE_POS_CONTROL(Ref_Valve_Pos);
+ break;
+ }
+
+ case MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING:
+ {
+
+ break;
+ }
+
+ case MODE_JOINT_POSITION_PRES_CONTROL_PWM:
+ {
+ joint_pos_err = Ref_Joint_Pos - (double) CUR_POSITION;
+ joint_pos_err_diff = joint_pos_err - joint_pos_err_old;
+ joint_pos_err_old = joint_pos_err;
+ joint_pos_err_sum += joint_pos_err;
+ if (joint_pos_err_sum > 1000) joint_pos_err_sum = 1000;
+ if (joint_pos_err_sum<-1000) joint_pos_err_sum = -1000;
+ 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;
+
+ joint_torq_err = Ref_Joint_Torq - CUR_TORQUE_NM;
+ joint_torq_err_diff = joint_torq_err - joint_torq_err_old;
+ joint_torq_err_old = joint_torq_err;
+ joint_torq_err_sum += joint_torq_err;
+ if (joint_torq_err_sum > 1000) joint_torq_err_sum = 1000;
+ if (joint_torq_err_sum<-1000) joint_torq_err_sum = -1000;
+ 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;
+
+ VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01;
+
+ VALVE_PWM_RAW = VALVE_PWM_RAW_POS + (double) COMPLIANCE_GAIN * 0.01 * VALVE_PWM_RAW_TORQ;
+
+ CUR_FLOWRATE = (double) CUR_VELOCITY * 0.00009587;
+ 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))
+ if (DIR_VALVE > 0) {
+ 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;
+ 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;
+ 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;
+ 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;
+ 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;
+ else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[8]*5.0)) VALVE_FF_VOLTAGE = 5.0;
+ 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;
+ 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;
+ 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;
+ 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;
+ 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;
+ else if (CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[9]*(-5.0))) VALVE_FF_VOLTAGE = -5;
+ else VALVE_FF_VOLTAGE = 0;
+ } else {
+ 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;
+ 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;
+ 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;
+ 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;
+ 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;
+ else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[9]*5.0)) VALVE_FF_VOLTAGE = 5.0;
+ 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;
+ 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;
+ 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;
+ 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;
+ 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;
+ else if (CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[8]*(-5.0))) VALVE_FF_VOLTAGE = -5;
+ else VALVE_FF_VOLTAGE = 0;
+ }
+ // VALVE_FF_VOLTAGE = CUR_FLOWRATE * 0.5;
+
+ 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.))
+ 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;
+
+ VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_FF_VOLTAGE * PWM_RESOL / SUPPLY_VOLTAGE;
+ break;
+ }
+
+ case MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION:
+ {
+ joint_pos_err = Ref_Joint_Pos - (double) CUR_POSITION;
+ joint_pos_err_diff = joint_pos_err - joint_pos_err_old;
+ joint_pos_err_old = joint_pos_err;
+ joint_pos_err_sum += joint_pos_err;
+ if (joint_pos_err_sum > 1000) joint_pos_err_sum = 1000;
+ if (joint_pos_err_sum<-1000) joint_pos_err_sum = -1000;
+ 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;
+
+ joint_torq_err = Ref_Joint_Torq - CUR_TORQUE_NM;
+ joint_torq_err_diff = joint_torq_err - joint_torq_err_old;
+ joint_torq_err_old = joint_torq_err;
+ joint_torq_err_sum += joint_torq_err;
+ if (joint_torq_err_sum > 1000) joint_torq_err_sum = 1000;
+ if (joint_torq_err_sum<-1000) joint_torq_err_sum = -1000;
+ 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;
+
+ Ref_Valve_Pos = VALVE_PWM_RAW_POS + VALVE_PWM_RAW_TORQ;
+
+ valve_pos_err = Ref_Valve_Pos - value;
+ valve_pos_err_diff = valve_pos_err - valve_pos_err_old;
+ valve_pos_err_old = valve_pos_err;
+ valve_pos_err_sum += valve_pos_err;
+ if (valve_pos_err_sum > 1000) valve_pos_err_sum = 1000;
+ if (valve_pos_err_sum<-1000) valve_pos_err_sum = -1000;
+
+ 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;
+
+ break;
+ }
+
+ case MODE_VALVE_POSITION_PRES_CONTROL_LEARNING:
+ {
+
+ break;
+ }
+
+ case MODE_TORQUE_SENSOR_NULLING:
+ {
+ // DAC Voltage reference set
+ if (TMR2_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) {
+ CUR_TORQUE_sum += CUR_TORQUE;
+
+ if (TMR2_COUNT_TORQUE_NULL % 10 == 0) {
+ CUR_TORQUE_mean = CUR_TORQUE_sum / 10.0;
+ CUR_TORQUE_sum = 0;
+
+ TORQUE_VREF += 0.0001 * (TORQUE_NULL - CUR_TORQUE_mean);
+
+ // TORQUE_VREF = 0;
+ if (TORQUE_VREF > 3.3) TORQUE_VREF = 3.3;
+ if (TORQUE_VREF < 0) TORQUE_VREF = 0;
+
+ spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0));
+ spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0));
+ }
+ } else {
+ CONTROL_MODE = MODE_NO_ACT;
+ TMR2_COUNT_TORQUE_NULL = 0;
+ CUR_TORQUE_sum = 0;
+ CUR_TORQUE_mean = 0;
+ spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0));
+
+ }
+ TMR2_COUNT_TORQUE_NULL++;
+ break;
+ }
+
+ case MODE_VALVE_NULLING_AND_DEADZONE_SETTING:
+ {
+ if (TMR2_COUNT_DEADZONE == 0) {
+ if (pos_plus_end == pos_minus_end) need_enc_init = true;
+ else temp_time = 0;
+ }
+ if (need_enc_init) {
+ if (TMR2_COUNT_DEADZONE < (int) (0.5 * (double) TMR_FREQ_5k)) {
+ VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+ pos_plus_end = CUR_POSITION;
+ } else if (TMR2_COUNT_DEADZONE < TMR_FREQ_5k) {
+ VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+ pos_minus_end = CUR_POSITION;
+ } else if (TMR2_COUNT_DEADZONE == TMR_FREQ_5k) need_enc_init = false;
+ temp_time = TMR_FREQ_5k;
+ }
+
+ if (temp_time <= TMR2_COUNT_DEADZONE && TMR2_COUNT_DEADZONE < (temp_time + TMR_FREQ_5k)) {
+ VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION);
+ VALVE_CENTER = VALVE_DEADZONE_PLUS = VALVE_DEADZONE_MINUS = 0;
+
+ } else if (temp_time <= TMR2_COUNT_DEADZONE && TMR2_COUNT_DEADZONE < (temp_time + (int) (1.9 * (double) TMR_FREQ_5k))) {
+ VALVE_PWM_RAW = 0;
+ CUR_VELOCITY_sum += CUR_VELOCITY;
+ } else if (TMR2_COUNT_DEADZONE == (temp_time + 2 * TMR_FREQ_5k)) {
+ if (CUR_VELOCITY_sum == 0) DZ_dir = 1;
+ else if (CUR_VELOCITY_sum > 0) DZ_dir = 1;
+ else if (CUR_VELOCITY_sum < 0) DZ_dir = -1;
+ else DZ_temp_cnt2 = DZ_end;
+ CUR_VELOCITY_sum = 0;
+ } else if (TMR2_COUNT_DEADZONE > (temp_time + 2 * TMR_FREQ_5k)) {
+ if (TMR2_COUNT_DEADZONE > (temp_time + 10 * TMR_FREQ_5k)) DZ_temp_cnt2 = DZ_end;
+
+ // Position of Dead Zone
+ // (CUR_VELOCITY < 0) (CUR_VELOCITY == 0) (CUR_VELOCITY > 0)
+ // | / | / |/
+ // | ______/ ___|___/ ______/|
+ // |/ / | / |
+ // /| / | / |
+ // 0V 0V 0V
+
+ if (DZ_temp_cnt2 < DZ_end) {
+ if (TMR2_COUNT_DEADZONE % 20 != 0) {
+ CUR_VELOCITY_sum += CUR_VELOCITY;
+ } else {
+ VALVE_PWM_RAW -= DZ_dir;
+ if (CUR_VELOCITY_sum * DZ_dir < 0) DZ_temp_cnt++;
+ CUR_VELOCITY_sum = 0;
+ }
+ if (DZ_temp_cnt == 5) {
+ if (DZ_dir >= 0) VALVE_DEADZONE_MINUS = (int16_t) VALVE_PWM_RAW;
+ else VALVE_DEADZONE_PLUS = (int16_t) VALVE_PWM_RAW;
+ DZ_dir = -DZ_dir;
+ DZ_temp_cnt = 0;
+ DZ_temp_cnt2++;
+ }
+ } else {
+ TMR2_COUNT_DEADZONE = -1;
+ VALVE_CENTER = VALVE_DEADZONE_PLUS / 2 + VALVE_DEADZONE_MINUS / 2;
+ if (VALVE_DEADZONE_PLUS < VALVE_DEADZONE_MINUS) {
+ VALVE_DEADZONE_PLUS = VALVE_CENTER;
+ VALVE_DEADZONE_MINUS = VALVE_CENTER;
+ }
+ VALVE_PWM_RAW = 0;
+ spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS);
+ spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS);
+
+ CONTROL_MODE = MODE_NO_ACT;
+ DZ_temp_cnt2 = 0;
+ }
+ }
+ TMR2_COUNT_DEADZONE++;
+ break;
+ }
+
+ case MODE_FIND_HOME:
+ {
+ if (FLAG_FIND_HOME == true) {
+ cnt_findhome = 0;
+ cnt_vel_findhome = 0;
+ REFERENCE_MODE = MODE_REF_NO_ACT; // Stop taking reference data from PODO
+ Ref_Joint_Pos = CUR_POSITION;
+ FLAG_FIND_HOME = false;
+ }
+
+ int check_enc = (TMR_FREQ_5k/10);
+ if(cnt_findhome%check_enc == 0){
+ FINDHOME_POSITION = CUR_POSITION;
+ FINDHOME_VELOCITY = FINDHOME_POSITION - FINDHOME_POSITION_OLD;
+ FINDHOME_POSITION_OLD = FINDHOME_POSITION;
+ } cnt_findhome++;
+ if(cnt_findhome == 10000) cnt_findhome = 0;
+
+ if (abs(FINDHOME_VELOCITY) <= 1) {
+ cnt_vel_findhome = cnt_vel_findhome + 1;
+ } else {
+ cnt_vel_findhome = 0;
+ }
+
+ if (cnt_vel_findhome < 2*TMR_FREQ_5k) {
+ REFERENCE_MODE = MODE_REF_NO_ACT;
+ if (HOMEPOS_OFFSET > 0) Ref_Joint_Pos = Ref_Joint_Pos + 1.0;
+ else Ref_Joint_Pos = Ref_Joint_Pos - 1.0;
+ joint_pos_err = Ref_Joint_Pos - CUR_POSITION;
+ VALVE_PWM_RAW = ((double) P_GAIN_JOINT_POSITION * joint_pos_err) * 0.01;
+ // if (HOMEPOS_OFFSET > 0) VALVE_PWM_RAW = 100;
+ // else VALVE_PWM_RAW = -100;
+ } else {
+ ENC_SET(HOMEPOS_OFFSET);
+ FLAG_REFERENCE_JOINT_POSITION = 1;
+ INIT_REF_POS = HOMEPOS_OFFSET;
+ REF_MOVE_TIME_5k = 2 * TMR_FREQ_5k;
+ TMR2_COUNT_REFERENCE = 0;
+ Ref_Joint_Vel = 0.0;
+ REFERENCE_MODE = MODE_REF_COS_INC;
+ CONTROL_MODE = MODE_JOINT_POSITION_TORQUE_CONTROL_PWM;
+
+ FINDHOME_POSITION = 0;
+ FINDHOME_POSITION_OLD = 0;
+ FINDHOME_VELOCITY = 0;
+ cnt_findhome = 0;
+ cnt_vel_findhome = 0;
+ }
+ break;
+ }
+
+ case MODE_VALVE_GAIN_SETTING:
+ {
+ if (TMR2_COUNT_FLOWRATE == 0) {
+ if (pos_plus_end == pos_minus_end) need_enc_init = true;
+ else {
+ VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+ temp_time = (int) (0.5 * (double) TMR_FREQ_5k);
+ }
+ }
+ if (need_enc_init) {
+ if (TMR2_COUNT_FLOWRATE < (int) (0.5 * (double) TMR_FREQ_5k)) {
+ VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+ pos_plus_end = CUR_POSITION;
+ } else if (TMR2_COUNT_FLOWRATE < TMR_FREQ_5k) {
+ VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+ pos_minus_end = CUR_POSITION;
+ } else if (TMR2_COUNT_FLOWRATE == TMR_FREQ_5k) {
+ need_enc_init = false;
+ check_vel_pos_init = (int) (0.9 * (double) (pos_plus_end - pos_minus_end));
+ check_vel_pos_fin = (int) (0.95 * (double) (pos_plus_end - pos_minus_end));
+ check_vel_pos_interv = check_vel_pos_fin - check_vel_pos_init;
+ }
+ temp_time = TMR_FREQ_5k;
+ }
+ TMR2_COUNT_FLOWRATE++;
+ if (TMR2_COUNT_FLOWRATE > temp_time) {
+ if (flag_flowrate % 2 == 0) { // (+)
+ VALVE_VOLTAGE = 1. * (double) (flag_flowrate / 2 + 1);
+ VALVE_PWM_RAW = VALVE_VOLTAGE / SUPPLY_VOLTAGE*PWM_RESOL;
+ if (CUR_POSITION > (pos_minus_end + check_vel_pos_init) && CUR_POSITION < (pos_minus_end + check_vel_pos_fin)) {
+ fl_temp_cnt++;
+ } else if (CUR_POSITION >= (pos_minus_end + check_vel_pos_fin) && CUR_VELOCITY == 0) {
+ 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
+ // VALVE_GAIN_LPM_PER_V[flag_flowrate] = (double) TMR_FREQ_10k * (double) check_vel_pos_interv / (double) fl_temp_cnt / VALVE_VOLTAGE; // PULSE/sec
+ fl_temp_cnt2++;
+ }
+ } else if (flag_flowrate % 2 == 1) { // (-)
+ VALVE_VOLTAGE = -1. * (double) (flag_flowrate / 2 + 1);
+ VALVE_PWM_RAW = VALVE_VOLTAGE / SUPPLY_VOLTAGE*PWM_RESOL;
+ if (CUR_POSITION < (pos_plus_end - check_vel_pos_init) && CUR_POSITION > (pos_plus_end - check_vel_pos_fin)) {
+ fl_temp_cnt++;
+ } else if (CUR_POSITION <= (pos_plus_end - check_vel_pos_fin) && CUR_VELOCITY == 0) {
+ 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);
+ // VALVE_GAIN_LPM_PER_V[flag_flowrate] = (double) TMR_FREQ_10k * (double) check_vel_pos_interv / (double) fl_temp_cnt / (-VALVE_VOLTAGE); // PULSE/sec
+ fl_temp_cnt2++;
+ }
+ }
+ if (fl_temp_cnt2 == 100) {
+ spi_eeprom_write(RID_VALVE_GAIN_PLUS_1 + flag_flowrate, (int16_t) (VALVE_GAIN_LPM_PER_V[flag_flowrate] * 100.0));
+ cur_vel_sum = 0;
+ fl_temp_cnt = 0;
+ fl_temp_cnt2 = 0;
+ flag_flowrate++;
+ }
+ if (flag_flowrate == 10) {
+ VALVE_PWM_RAW = 0;
+ flag_flowrate = 0;
+ TMR2_COUNT_FLOWRATE = 0;
+ valve_gain_repeat_cnt++;
+ if (valve_gain_repeat_cnt >= 1) {
+ CONTROL_MODE = MODE_NO_ACT;
+ valve_gain_repeat_cnt = 0;
+ }
+
+ }
+ break;
+ }
+
+ }
+
+ case MODE_PRESSURE_SENSOR_NULLING:
+ {
+ // DAC Voltage reference set
+ if (TMR2_COUNT_PRES_NULL < TMR_FREQ_5k * 2) {
+ CUR_PRES_A_sum += CUR_PRES_A;
+ CUR_PRES_B_sum += CUR_PRES_B;
+
+ if (TMR2_COUNT_PRES_NULL % 10 == 0) {
+ CUR_PRES_A_mean = CUR_PRES_A_sum / 10.0;
+ CUR_PRES_B_mean = CUR_PRES_B_sum / 10.0;
+ CUR_PRES_A_sum = 0;
+ CUR_PRES_B_sum = 0;
+
+ double VREF_NullingGain = 0.0003;
+ PRES_A_VREF -= VREF_NullingGain * (PRES_A_NULL - CUR_PRES_A_mean);
+ PRES_B_VREF -= VREF_NullingGain * (PRES_B_NULL - CUR_PRES_B_mean);
+
+ if (PRES_A_VREF > 3.3) PRES_A_VREF = 3.3;
+ if (PRES_A_VREF < 0) PRES_A_VREF = 0;
+ if (PRES_B_VREF > 3.3) PRES_B_VREF = 3.3;
+ if (PRES_B_VREF < 0) PRES_B_VREF = 0;
+
+ spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0));
+ spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0));
+ }
+ } else {
+ CONTROL_MODE = MODE_NO_ACT;
+ TMR2_COUNT_PRES_NULL = 0;
+ CUR_PRES_A_sum = 0;
+ CUR_PRES_B_sum = 0;
+ CUR_PRES_A_mean = 0;
+ CUR_PRES_B_mean = 0;
+
+ spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0));
+ spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0));
+ }
+ TMR2_COUNT_PRES_NULL++;
+ break;
+ }
+
+ case MODE_PRESSURE_SENSOR_CALIB:
+ {
+ if (TMR2_COUNT_PRES_CALIB < 2 * TMR_FREQ_5k) {
+ VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL / 5.;
+ if (TMR2_COUNT_PRES_CALIB >= TMR_FREQ_5k) {
+ CUR_PRES_A_sum += CUR_PRES_A;
+ }
+ } else if (TMR2_COUNT_PRES_CALIB < 4 * TMR_FREQ_5k) {
+ VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL / 5.;
+ if (TMR2_COUNT_PRES_CALIB >= 3 * TMR_FREQ_5k) {
+ CUR_PRES_B_sum += CUR_PRES_B;
+ }
+ } else {
+ CONTROL_MODE = MODE_NO_ACT;
+ TMR2_COUNT_PRES_CALIB = 0;
+ VALVE_PWM_RAW = 0;
+ PRES_SENSOR_A_PULSE_PER_BAR = CUR_PRES_A_sum / ((double) TMR_FREQ_5k - 1.) - PRES_A_NULL;
+ PRES_SENSOR_A_PULSE_PER_BAR = PRES_SENSOR_A_PULSE_PER_BAR / ((double) PRES_SUPPLY - 1.);
+ PRES_SENSOR_B_PULSE_PER_BAR = CUR_PRES_B_sum / ((double) TMR_FREQ_5k - 1.) - PRES_B_NULL;
+ PRES_SENSOR_B_PULSE_PER_BAR = PRES_SENSOR_B_PULSE_PER_BAR / ((double) PRES_SUPPLY - 1.);
+ CUR_PRES_A_sum = 0;
+ CUR_PRES_B_sum = 0;
+ CUR_PRES_A_mean = 0;
+ CUR_PRES_B_mean = 0;
+
+ spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0));
+ spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0));
+ }
+ TMR2_COUNT_PRES_CALIB++;
+ break;
+ }
+
+ case MODE_ROTARY_FRICTION_TUNING:
+ {
+ 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);
+ VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL * sin(2 * 3.14159 * freq_fric_tune * TMR2_COUNT_ROTARY_FRIC_TUNE * 0.0001);
+ if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL;
+ else VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+ TMR2_COUNT_ROTARY_FRIC_TUNE++;
+ if (TMR2_COUNT_ROTARY_FRIC_TUNE > TUNING_TIME * TMR_FREQ_5k) {
+ TMR2_COUNT_ROTARY_FRIC_TUNE = 0;
+ VALVE_PWM_RAW = 0;
+ CONTROL_MODE = MODE_NO_ACT;
+ }
+ break;
+ }
+
+ case MODE_DDV_POS_VS_PWM_ID:
+ {
+ VALVE_ID_timer = VALVE_ID_timer + 1;
+
+ if(VALVE_ID_timer < TMR_FREQ_5k*1)
+ {
+ VALVE_PWM_RAW = 3000 * sin(2*3.14*VALVE_ID_timer/TMR_FREQ_5k * 100);
+ }
+ else if(VALVE_ID_timer < TMR_FREQ_5k*2)
+ {
+ VALVE_PWM_RAW = 1000*(ID_index_array[ID_index]);
+ }
+ else if(VALVE_ID_timer == TMR_FREQ_5k*2)
+ {
+ VALVE_POS_TMP = 0;
+ data_num = 0;
+ }
+ else if(VALVE_ID_timer < TMR_FREQ_5k*3)
+ {
+ data_num = data_num + 1;
+ VALVE_POS_TMP = VALVE_POS_TMP + value;
+ }
+ else if(VALVE_ID_timer == TMR_FREQ_5k*3)
+ {
+ VALVE_POS_AVG[ID_index] = VALVE_POS_TMP / data_num;
+ VALVE_ID_timer = 0;
+ ID_index= ID_index +1;
+ }
+
+ if(ID_index>=18)
+ {
+ int i;
+ VALVE_POS_AVG_OLD = VALVE_POS_AVG[0];
+ for(i=0;i<18;i++)
+ {
+ VALVE_POS_VS_PWM[i] = (int16_t) (VALVE_POS_AVG[i]);
+ spi_eeprom_write(RID_VALVE_POS_VS_PWM_0 + i, (int16_t) (VALVE_POS_AVG[i]));
+ if(VALVE_POS_AVG[i] > VALVE_POS_AVG_OLD)
+ {
+ VALVE_MAX_POS = VALVE_POS_AVG[i];
+ VALVE_POS_AVG_OLD = VALVE_MAX_POS;
+ }
+ else if(VALVE_POS_AVG[i] < VALVE_POS_AVG_OLD)
+ {
+ VALVE_MIN_POS = VALVE_POS_AVG[i];
+ VALVE_POS_AVG_OLD = VALVE_MIN_POS;
+ }
+ }
+ spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) (VALVE_MAX_POS));
+ spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) (VALVE_MIN_POS));
+ CAN_TX_PRES((int16_t) (VALVE_MAX_POS), (int16_t) (VALVE_MIN_POS));
+ ID_index = 0;
+ CONTROL_MODE = MODE_NO_ACT;
+ }
+
+
+ break;
+ }
+
+ case MODE_DDV_DEADZONE_AND_CENTER:
+ {
+
+ VALVE_DZ_timer = VALVE_DZ_timer + 1;
+ if(first_check == 0)
+ {
+ if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k))
+ {
+ VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+ }
+ else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k))
+ {
+ VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+ pos_plus_end = CUR_POSITION;
+ }
+ else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k))
+ {
+ VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+ }
+ else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k))
+ {
+ VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+ pos_minus_end = CUR_POSITION;
+ }
+ else if(VALVE_DZ_timer < (int) (3.0 * (double) TMR_FREQ_5k))
+ {
+ 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);
+ }
+ else if(VALVE_DZ_timer < (int) (4.0 * (double) TMR_FREQ_5k))
+ {
+ 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);
+ data_num = data_num + 1;
+ VALVE_POS_TMP = VALVE_POS_TMP + value;
+ }
+ else if(VALVE_DZ_timer == (int) (4.0 * (double) TMR_FREQ_5k))
+ {
+ 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);
+ DDV_POS_AVG = VALVE_POS_TMP / data_num;
+ START_POS = CUR_POSITION;
+ CAN_TX_PRES((int16_t) (DDV_POS_AVG), (int16_t) (data_num));
+ VALVE_POS_TMP = 0;
+ data_num = 0;
+
+ }
+ else if(VALVE_DZ_timer < (int) (5.0 * (double) TMR_FREQ_5k))
+ {
+ Ref_Valve_Pos = DDV_POS_AVG;
+ VALVE_POS_CONTROL(Ref_Valve_Pos);
+ //CAN_TX_PRES((int16_t) (VALVE_MAX_POS), (int16_t) (VALVE_MIN_POS));
+
+ }
+ else if(VALVE_DZ_timer < (int) (6.0 * (double) TMR_FREQ_5k))
+ {
+ Ref_Valve_Pos = DDV_POS_AVG;
+ VALVE_POS_CONTROL(Ref_Valve_Pos);
+ if(CUR_VELOCITY >= 0)
+ VEL_POINT = VEL_POINT + 1;
+ else
+ VEL_POINT = VEL_POINT - 1;
+ }
+ else if(VALVE_DZ_timer == (int) (6.0 * (double) TMR_FREQ_5k))
+ {
+ Ref_Valve_Pos = DDV_POS_AVG;
+ VALVE_POS_CONTROL(Ref_Valve_Pos);
+ FINAL_POS = CUR_POSITION;
+ // if(VEL_POINT >= 0)
+ // {
+ // DZ_case = 1;
+ // }
+ // else
+ // {
+ // DZ_case = -1;
+ // }
+ // if(abs(FINAL_POS - START_POS) < 100)
+ // {
+ // DZ_case = 0;
+ // }
+
+ if((FINAL_POS - START_POS)>100)
+ {
+ DZ_case = 1;
+ }
+ else if((FINAL_POS - START_POS)<-100)
+ {
+ DZ_case = -1;
+ }
+ else
+ {
+ DZ_case = 0;
+ }
+
+ VEL_POINT = 0;
+ first_check = 1;
+ DZ_DIRECTION = 1;
+ VALVE_DZ_timer = 0;
+ Ref_Valve_Pos_Old = DDV_POS_AVG;
+ DZ_NUM = 1;
+ DZ_index = 1;
+ CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (DZ_NUM));
+
+ }
+ }
+ else
+ {
+ if((DZ_case == -1 && DZ_NUM == 1) | (DZ_case == 1 && DZ_NUM == 1))
+ {
+ if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k))
+ {
+ 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);
+ }
+ else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k))
+ {
+ START_POS = CUR_POSITION;
+ }
+ else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k))
+ {
+ Ref_Valve_Pos = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index;
+ if(Ref_Valve_Pos <= VALVE_MIN_POS)
+ {
+ Ref_Valve_Pos = VALVE_MIN_POS;
+ }
+ else if(Ref_Valve_Pos >= VALVE_MAX_POS)
+ {
+ Ref_Valve_Pos = VALVE_MAX_POS;
+ }
+ VALVE_POS_CONTROL(Ref_Valve_Pos);
+ if(CUR_VELOCITY >= 0)
+ VEL_POINT = VEL_POINT + 1;
+ else
+ VEL_POINT = VEL_POINT - 1;
+ }
+ else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k))
+ {
+ Ref_Valve_Pos_Old = Ref_Valve_Pos;
+ FINAL_POS = CUR_POSITION;
+ // if(VEL_POINT >= 0)
+ // DZ_DIRECTION = 1 * DZ_case;
+ // else
+ // DZ_DIRECTION = -1 * DZ_case;
+ // if(abs(FINAL_POS - START_POS) < 100)
+ // DZ_DIRECTION = 1 * DZ_case;
+
+ if((FINAL_POS - START_POS)>100)
+ {
+ DZ_DIRECTION = 1 * DZ_case;
+ }
+ else if((FINAL_POS - START_POS)<-100)
+ {
+ DZ_DIRECTION = -1 * DZ_case;
+ }
+ else
+ {
+ DZ_DIRECTION = 1 * DZ_case;
+ }
+
+ VEL_POINT = 0;
+
+ VALVE_DZ_timer = 0;
+ DZ_index= DZ_index *2;
+ if(DZ_index >= 128)
+ {
+ FIRST_DZ = Ref_Valve_Pos;
+ DZ_NUM = 2;
+ Ref_Valve_Pos_Old = FIRST_DZ;
+ DZ_index = 1;
+ DZ_DIRECTION = 1;
+ }
+
+ //CAN_TX_PRES((int16_t) (1), (int16_t) (DZ_index));
+ }
+ }
+ else if((DZ_case == -1 && DZ_NUM == 2) | (DZ_case == 1 && DZ_NUM == 2))
+ {
+ if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k))
+ {
+ 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);
+ }
+ else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k))
+ {
+ START_POS = CUR_POSITION;
+ }
+ else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k))
+ {
+ Ref_Valve_Pos = Ref_Valve_Pos_Old + DZ_DIRECTION + 64 / DZ_index;
+ if(Ref_Valve_Pos <= VALVE_MIN_POS)
+ {
+ Ref_Valve_Pos = VALVE_MIN_POS;
+ }
+ else if(Ref_Valve_Pos >= VALVE_MAX_POS)
+ {
+ Ref_Valve_Pos = VALVE_MAX_POS;
+ }
+ VALVE_POS_CONTROL(Ref_Valve_Pos);
+ if(CUR_VELOCITY >= 0)
+ VEL_POINT = VEL_POINT + 1;
+ else
+ VEL_POINT = VEL_POINT - 1;
+ }
+ else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k))
+ {
+ Ref_Valve_Pos_Old = Ref_Valve_Pos;
+ FINAL_POS = CUR_POSITION;
+ // if(VEL_POINT >= 0)
+ // DZ_DIRECTION = 1 * DZ_case;
+ // else
+ // DZ_DIRECTION = -1 * DZ_case;
+ // if(abs(FINAL_POS - START_POS) < 100)
+ // DZ_DIRECTION = -1 * DZ_case;
+
+ if((FINAL_POS - START_POS)>100)
+ {
+ DZ_DIRECTION = -1;
+ }
+ else if((FINAL_POS - START_POS)<-100)
+ {
+ DZ_DIRECTION = 1;
+ }
+ else
+ {
+ DZ_DIRECTION = 1;
+ }
+
+ VEL_POINT = 0;
+
+
+ VALVE_DZ_timer = 0;
+ DZ_index= DZ_index *2;
+ if(DZ_index >= 128)
+ {
+ SECOND_DZ = Ref_Valve_Pos;
+ DDV_CENTER = (int) (0.5 * (double) (FIRST_DZ) + 0.5 * (double) (SECOND_DZ));
+ first_check = 0;
+ spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ);
+ spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ);
+ spi_eeprom_write(RID_DDV_CNETER, DDV_CENTER);
+ //CAN_TX_PRES((int16_t) FIRST_DZ, (int16_t) SECOND_DZ);
+ CONTROL_MODE = MODE_NO_ACT;
+ DZ_index = 1;
+ }
+
+ CAN_TX_PRES((int16_t) (2), (int16_t) (DZ_index));
+ }
+ }
+ else if(DZ_case == 0 && DZ_NUM ==1)
+ {
+ if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k))
+ {
+ 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);
+ }
+ else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k))
+ {
+ START_POS = CUR_POSITION;
+ }
+ else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k))
+ {
+ Ref_Valve_Pos = Ref_Valve_Pos_Old - DZ_DIRECTION * 64 / DZ_index;
+ if(Ref_Valve_Pos <= VALVE_MIN_POS)
+ {
+ Ref_Valve_Pos = VALVE_MIN_POS;
+ }
+ else if(Ref_Valve_Pos >= VALVE_MAX_POS)
+ {
+ Ref_Valve_Pos = VALVE_MAX_POS;
+ }
+ VALVE_POS_CONTROL(Ref_Valve_Pos);
+ if(CUR_VELOCITY >= 0)
+ VEL_POINT = VEL_POINT + 1;
+ else
+ VEL_POINT = VEL_POINT - 1;
+ }
+ else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k))
+ {
+ Ref_Valve_Pos_Old = Ref_Valve_Pos;
+ FINAL_POS = CUR_POSITION;
+ // if(VEL_POINT >= 0)
+ // DZ_DIRECTION = 1;
+ // else
+ // DZ_DIRECTION = -1;
+ // if(abs(FINAL_POS - START_POS) < 100)
+ // DZ_DIRECTION = 1;
+
+ if((FINAL_POS - START_POS)>100)
+ {
+ DZ_DIRECTION = 1;
+ }
+ else if((FINAL_POS - START_POS)<-100)
+ {
+ DZ_DIRECTION = -1;
+ }
+ else
+ {
+ DZ_DIRECTION = 1;
+ }
+
+ VEL_POINT = 0;
+ //CAN_TX_PRES((int16_t) (7), (int16_t) (DZ_DIRECTION));
+ VALVE_DZ_timer = 0;
+ DZ_index= DZ_index *2;
+ if(DZ_index >= 128)
+ {
+ FIRST_DZ = Ref_Valve_Pos;
+ DZ_NUM = 2;
+ Ref_Valve_Pos_Old = FIRST_DZ;
+ DZ_index = 1;
+ DZ_DIRECTION = 1;
+ }
+ }
+ }
+ else
+ {
+ if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k))
+ {
+ 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);
+ }
+ else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k))
+ {
+ START_POS = CUR_POSITION;
+ }
+ else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k))
+ {
+ Ref_Valve_Pos = Ref_Valve_Pos_Old + DZ_DIRECTION * 64 / DZ_index;
+ if(Ref_Valve_Pos <= VALVE_MIN_POS)
+ {
+ Ref_Valve_Pos = VALVE_MIN_POS;
+ }
+ else if(Ref_Valve_Pos > VALVE_MAX_POS)
+ {
+ Ref_Valve_Pos = VALVE_MAX_POS - 1;
+ }
+ VALVE_POS_CONTROL(Ref_Valve_Pos);
+ if(CUR_VELOCITY >= 0)
+ VEL_POINT = VEL_POINT + 1;
+ else
+ VEL_POINT = VEL_POINT - 1;
+ }
+ else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k))
+ {
+ Ref_Valve_Pos_Old = Ref_Valve_Pos;
+ FINAL_POS = CUR_POSITION;
+ if(VEL_POINT >= 0)
+ DZ_DIRECTION = -1;
+ else
+ DZ_DIRECTION = 1;
+ if(abs(FINAL_POS - START_POS) < 100)
+ DZ_DIRECTION = 1;
+
+ VEL_POINT = 0;
+
+
+ VALVE_DZ_timer = 0;
+ //CAN_TX_PRES((int16_t) (3), (int16_t) (DZ_index));
+ DZ_index= DZ_index *2;
+ if(DZ_index >= 128)
+ {
+ SECOND_DZ = Ref_Valve_Pos;
+ DDV_CENTER = (int) (0.5 * (double) (FIRST_DZ) + 0.5 * (double) (SECOND_DZ));
+ first_check = 0;
+ VALVE_DEADZONE_MINUS = FIRST_DZ;
+ spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ);
+ VALVE_DEADZONE_PLUS = SECOND_DZ;
+ spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ);
+ spi_eeprom_write(RID_DDV_CNETER, DDV_CENTER);
+ CAN_TX_PRES((int16_t) FIRST_DZ, (int16_t) SECOND_DZ);
+ CONTROL_MODE = MODE_NO_ACT;
+ DZ_index = 1;
+ }
+
+ //CAN_TX_PRES((int16_t) (4), (int16_t) (DZ_index));
+ }
+ }
+ }
+ break;
+ }
+
+ case MODE_DDV_POS_VS_FLOWRATE:
+ {
+ VALVE_FR_timer = VALVE_FR_timer + 1;
+ if(first_check == 0)
+ {
+ if(VALVE_FR_timer < (int) (1.0 * (double) TMR_FREQ_5k))
+ {
+ VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+ CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6));
+ }
+ else if(VALVE_FR_timer == (int) (1.0 * (double) TMR_FREQ_5k))
+ {
+ VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+ pos_plus_end = CUR_POSITION;
+ // CAN_TX_PRES((int16_t) (VALVE_PWM_RAW), (int16_t) (7));
+ }
+ else if(VALVE_FR_timer < (int) (2.0 * (double) TMR_FREQ_5k))
+ {
+ VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+ }
+ else if(VALVE_FR_timer == (int) (2.0 * (double) TMR_FREQ_5k))
+ {
+ // CAN_TX_PRES((int16_t) (VALVE_PWM_RAW), (int16_t) (8));
+ VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+ pos_minus_end = CUR_POSITION;
+ first_check = 1;
+ VALVE_FR_timer = 0;
+ Ref_Valve_Pos = DDV_CENTER;
+ ID_index = 0;
+ max_check = 0;
+ min_check = 0;
+ }
+ }
+ else
+ {
+ if(VALVE_FR_timer < (int) (1.0 * (double) TMR_FREQ_5k))
+ {
+ 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);
+ }
+ else if(VALVE_FR_timer == (int) (1.0 * (double) TMR_FREQ_5k))
+ {
+ data_num = 0;
+ Ref_Valve_Pos = 50*(ID_index_array[ID_index]) + DDV_CENTER;
+
+ VALVE_POS_CONTROL(Ref_Valve_Pos);
+ START_POS = CUR_POSITION;
+ }
+ else if(VALVE_FR_timer < (int) (5.0 * (double) TMR_FREQ_5k))
+ {
+ Ref_Valve_Pos = 50*(ID_index_array[ID_index]) + DDV_CENTER;
+ VALVE_POS_CONTROL(Ref_Valve_Pos);
+ data_num = data_num + 1;
+ if(abs(0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION) > 20000)
+ {
+ FINAL_POS = CUR_POSITION;
+ one_period_end = 1;
+ }
+ }
+ else if(VALVE_FR_timer == (int) (5.0 * (double) TMR_FREQ_5k))
+ {
+ FINAL_POS = CUR_POSITION;
+ one_period_end = 1;
+ }
+
+ if(one_period_end == 1){
+ if(Ref_Valve_Pos > VALVE_MAX_POS)
+ {
+ max_check = 1;
+ }
+ else if(Ref_Valve_Pos < VALVE_MIN_POS)
+ {
+ min_check = 1;
+ }
+ JOINT_VEL[ID_index] = (FINAL_POS - START_POS) / data_num * TMR_FREQ_5k; // pulse/sec
+ spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0 + ID_index, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF));
+ spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0_1 + ID_index, (int16_t) ((JOINT_VEL[ID_index] >> 16) & 0xFFFF));
+ VALVE_FR_timer = 0;
+ one_period_end = 0;
+ ID_index= ID_index +1;
+ // CAN_TX_PRES((int16_t) (Ref_Valve_Pos), (int16_t) (ID_index));
+ }
+
+ if(max_check == 1 && min_check == 1)
+ {
+ spi_eeprom_write(RID_VALVE_POS_NUM, (int16_t) (ID_index));
+ VALVE_POS_NUM = ID_index;
+ ID_index = 0;
+ first_check = 0;
+ VALVE_FR_timer = 0;
+ // CAN_TX_PRES((int16_t) (123), (int16_t) (123));
+ CONTROL_MODE = MODE_NO_ACT;
+ }
+ }
+ break;
+ }
default:
break;
}
+ if (FLAG_VALVE_OUTPUT_CALIB) {
+ // Valve Dead Zone (Mechanical dead-zone canceling)
+ // if (CONTROL_MODE != MODE_VALVE_NULLING_AND_DEADZONE_SETTING) {
+ // if (VALVE_PWM_RAW > 0.) {
+ // VALVE_PWM_VALVE_DZ = (int) VALVE_PWM_RAW + VALVE_DEADZONE_PLUS;
+ // } else if (VALVE_PWM_RAW < 0.) {
+ // VALVE_PWM_VALVE_DZ = (int) VALVE_PWM_RAW + VALVE_DEADZONE_MINUS;
+ // } else VALVE_PWM_VALVE_DZ = (int) VALVE_PWM_RAW + VALVE_CENTER;
+ // } else VALVE_PWM_VALVE_DZ = (int) VALVE_PWM_RAW;
+
+ if(VALVE_PWM_RAW>0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS;
+ else if(VALVE_PWM_RAW<0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS;
+
+ VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + VALVE_CENTER;
+ // Output Voltage Linearization and Dead Zone Cancellation (Electrical dead-zone)
+ double CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ/PWM_RESOL*SUPPLY_VOLTAGE*1000.0; // convert PWM duty to mV
+ double CUR_PWM_DZ = PWM_duty_byLT(CUR_PWM_nonlin);
+
+ if (CUR_PWM_DZ > 0) CUR_PWM = (int)CUR_PWM_DZ + 143;
+ else if (CUR_PWM_DZ < 0) CUR_PWM = (int)CUR_PWM_DZ - 138;
+ else CUR_PWM = CUR_PWM_DZ;
+ } else {
+ CUR_PWM = VALVE_PWM_RAW;
+ }
+
+ VALVE_PWM(CUR_PWM, VALVE_VOLTAGE_LIMIT, SUPPLY_VOLTAGE);
+ //CAN ----------------------------------------------------------------------
+ if (TMR3_COUNT_CAN_TX % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
+
+ if (flag_data_request[0] == HIGH) {
+ //position+velocity
+ CAN_TX_POSITION((long) CUR_POSITION, (long) CUR_VELOCITY);
+ // CAN_TX_POSITION((int) (PRES_A_VREF * 100.), (int) (PRES_B_VREF * 100.));
+ // CAN_TX_POSITION((long) CUR_PRES_A_BAR, (long) CUR_PRES_B_BAR);
+ }
+
+ if (flag_data_request[1] == HIGH) {
+ //torque
+ //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM * 100.));
+ CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM));
+ // CAN_TX_TORQUE((int16_t) DZ_temp_cnt);
+ }
+
+ if (flag_data_request[2] == HIGH) {
+ //pressure A and B
+ CAN_TX_PRES((int16_t) (CUR_PRES_A), (int16_t) (CUR_PRES_B)); // CUR_PRES_X : 0(0bar)~4096(210bar)
+ // CAN_TX_PRES((int16_t) (CUR_PRES_A_BAR * 100.), (int16_t) (CUR_PRES_B_BAR * 100.));
+ // CAN_TX_PRES((int16_t) ((DEADZONE_MINUS + 1.)*1000.), (int16_t) ((DEADZONE_PLUS + 1.))*1000.);
+ // CAN_TX_PRES((int16_t) DZ_dir, (int16_t) ((VALVE_DEADZONE_PLUS + 1.))*1000.);
+
+ }
+
+ if (flag_data_request[3] == HIGH) {
+ //PWM
+ CAN_TX_PWM((int16_t) CUR_PWM);
+ // CAN_TX_PWM((int16_t) (CONTROL_MODE));
+ // CAN_TX_PWM((int16_t) cnt_vel_findhome);
+ // CAN_TX_PWM((int16_t) (VALVE_VOLTAGE * 1000.));
+ // CAN_TX_PWM((int16_t) (VALVE_VOLTAGE_VALVE_DZ * 1000.));
+
+ }
+
+ if (flag_data_request[4] == HIGH) {
+ //valve position
+ CAN_TX_VALVE_POSITION((int16_t) (I2C_val));
+ //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF));
+ //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0_1, (int16_t) ((0xFEF1>>16) & 0xFFFF));
+
+
+ //CAN_TX_VALVE_POSITION((int16_t) (VALVE_POS_NUM));
+ // CAN_TX_VALVE_POSITIOfxN((int16_t) (VALVE_FF_VOLTAGE * PWM_RESOL / SUPPLY_VOLTAGE));
+ // CAN_TX_VALVE_POSITION((int16_t) P_GAIN_JOINT_POSITION);
+ // CAN_TX_VALVE_POSITION((int16_t) Ref_Joint_Pos);
+ // CAN_TX_VALVE_POSITION((int16_t) flag_flowrate);
+ }
+
+ TMR3_COUNT_CAN_TX = 0;
+
+ }
+ TMR3_COUNT_CAN_TX++;
+
/*******************************************************
- *** Valve Control
+ *** Valve Control
********************************************************/
ValveControl(CONTROL_MODE);
-
+
double t = (double)CNT_TMR4*DT_TMR4;
- double T = 5.0;
+ double T = 5.0;
V_out = 1000.0*sin(2.0*PI*t/T); // V_out : -5000.0mV~5000.0mV(full duty)
// if(V_out > 0.0) V_out = 1000.0;
-// else if(V_out < 0.0) V_out = -1000.0;
+// else if(V_out < 0.0) V_out = -1000.0;
/*******************************************************
*** PWM
********************************************************/
PWM_out= V_out/12000.0; // Full duty : 12000.0mV
-
+
// Saturation of output voltage to 5.0V
if(PWM_out > 0.41667) PWM_out=0.41667; //5.0/12.0 = 0.41667
else if (PWM_out < -0.41667) PWM_out=-0.41667;
-
+
if (PWM_out>0.0) {
dtc_v=0.0;
dtc_w=PWM_out;
@@ -413,11 +1626,11 @@
dtc_v=-PWM_out;
dtc_w=0.0;
}
-
+
//pwm
TIM4->CCR2 = (PWM_ARR)*(1.0-dtc_v);
TIM4->CCR1 = (PWM_ARR)*(1.0-dtc_w);
-
+
/*******************************************************
*** Data Send (CAN) & Print out (UART)
********************************************************/
@@ -433,12 +1646,10 @@
// can.write(msg);
// }
- if((CNT_TMR3%5000)==0){
- if(LED==1)
- {
+ if((CNT_TMR3%5000)==0) {
+ if(LED==1) {
LED=0;
- }
- else
+ } else
LED = 1;
// LED != LED;
}
@@ -446,7 +1657,7 @@
/*******************************************************
*** Timer Counting & etc.
********************************************************/
- CNT_TMR3++;
+ CNT_TMR3++;
}
TIM3->SR = 0x0; // reset the status register
}
@@ -459,17 +1670,17 @@
break;
case MODE_VALVE_OPEN_LOOP: // 1
V_out = Vout.ref;
- break;
+ break;
case MODE_VALVE_POSITION_CONTROL: // 2
CurrentControl();
- break;
+ break;
case MODE_JOINT_POSITION_TORQUE_CONTROL_PWM: // 3
V_out = 0.0;
- break;
+ break;
case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION: // 4
double I_REF_POS_FB = 0.0; // I_REF by Position Feedback
double I_REF_POS_FF = 0.0; // I_REF by Position Feedforward
-
+
// feedback input for position control
pos.err = pos.ref - pos.sen;
double alpha_update_vel = 1.0/(1.0+(double)FREQ_TMR4/(2.0*3.1415*50.0)); // f_cutoff : 50Hz
@@ -477,34 +1688,35 @@
pos.err_diff = (1.0-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff;
pos.err_old = pos.err;
I_REF_POS_FB = 0.001*((double)P_GAIN_JOINT_POSITION * pos.err + (double)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1);
-
+
// feedforward input for position control
- double Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
+ double Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
double K_ff = 1.3;
double K_v = 0.0;
if(Vel_Act_Ref > 0) K_v = 1.0/100.0; // open, tuning. (deg/s >> mA)
- if(Vel_Act_Ref < 0) K_v = 1.0/100.0; // close, tuning. (deg/s >> mA)
+ if(Vel_Act_Ref < 0) K_v = 1.0/100.0; // close, tuning. (deg/s >> mA)
I_REF_POS_FF = K_ff*K_v*Vel_Act_Ref;
-
+
cur.ref = I_REF_POS_FF + I_REF_POS_FB;
- break;
+ break;
case MODE_TEST_CURRENT_CONTROL: // 9
V_out = 0.0;
- break;
+ break;
case MODE_TEST_PWM_CONTROL: // 10
V_out = 0.0;
- break;
+ break;
case MODE_FIND_HOME: // 22
V_out = 0.0;
- break;
+ break;
default:
V_out = 0.0;
break;
-
+
}
}
-void CurrentControl() {
+void CurrentControl()
+{
cur.err = cur.ref - cur.sen;
cur.err_int = cur.err_int + cur.err*DT_TMR4;
cur.err_diff = (cur.err - cur.err_old)*FREQ_TMR4;
@@ -521,8 +1733,8 @@
V_out = (int) (KP_I * cur.err + KI_I * cur.err_int + KD_I * cur.err_diff);
// V_out = V_out + FF_gain * (R_model*I_REF); // Unit : mV
V_out = V_out + FF_gain * (R_model*cur.ref + L_model*cur.ref_diff); // Unit : mV
-
- double Ka = 5.0/KP_I;
+
+ double Ka = 5.0/KP_I;
if(V_out > V_MAX) {
V_rem = V_out-V_MAX;
V_rem = Ka*V_rem;
@@ -530,8 +1742,8 @@
cur.err_int = cur.err_int - V_rem*DT_5k;
} else if(V_out < -V_MAX) {
V_rem = V_out-(-V_MAX);
- V_rem = Ka*V_rem;
+ V_rem = Ka*V_rem;
V_out = -V_MAX;
cur.err_int = cur.err_int - V_rem*DT_5k;
- }
+ }
}
\ No newline at end of file
--- a/setting.h Tue Aug 27 05:32:44 2019 +0000 +++ b/setting.h Tue Aug 27 06:50:54 2019 +0000 @@ -264,3 +264,97 @@ extern int MODE_JUMP_STATUS; +extern double CUR_PRES_DIFF_BAR; +extern double CUR_PRES_A_sum; +extern double CUR_PRES_B_sum; +extern double CUR_PRES_A_mean; +extern double CUR_PRES_B_mean; +extern double CUR_TORQUE_sum; +extern double CUR_TORQUE_mean; +extern double PRES_A_NULL; +extern double PRES_B_NULL; +extern double TORQUE_NULL; + +extern double Ref_PWM; +extern double Ref_Valve_Pos; +extern double Ref_Joint_Pos; +extern double Ref_Joint_Vel; +extern double Ref_Joint_Torq; +extern double Ref_Valve_Pos_Old; + +extern int VALVE_ID_timer; +extern int VALVE_DZ_timer; +extern int VALVE_FR_timer; +extern int VALVE_POS_TMP; +extern int JOINT_VEL_TMP; +extern int DDV_POS_AVG ; +extern int VALVE_POS_AVG[100]; +extern int VALVE_POS_AVG_OLD; +extern int data_num; +extern int ID_index; +extern int DZ_index; +extern int ID_index_array[100]; +extern int FR_index_array[100]; +extern int first_check; +extern double init_time; +extern int VEL_POINT; +extern int DZ_case; +extern int START_POS; +extern int FINAL_POS; +extern int DZ_DIRECTION; +extern int FIRST_DZ; +extern int SECOND_DZ; +extern int DZ_NUM; +extern int one_period_end; +extern double Ref_Vel_Test; +extern long TMR2_FOR_SLOW_LOGGING; +extern int velcount; +extern char max_check; +extern char min_check; + +extern double valve_pos_err, valve_pos_err_old, valve_pos_err_diff, valve_pos_err_sum; +extern double joint_pos_err, joint_pos_err_old, joint_pos_err_diff, joint_pos_err_sum; +extern double joint_torq_err, joint_torq_err_old, joint_torq_err_diff, joint_torq_err_sum; +extern double VALVE_PWM_RAW_POS, VALVE_PWM_RAW_TORQ; + +extern double CUR_FLOWRATE; +extern double VALVE_FF_VOLTAGE; + +extern int pos_plus_end; +extern int pos_minus_end; + +extern bool need_enc_init; + +extern int temp_time; + +extern double CUR_VELOCITY_sum; +extern double temp_vel_sum; + +extern int DZ_dir; +extern int DZ_temp_cnt; +extern int DZ_temp_cnt2; +extern int DZ_end; +extern int flag_flowrate; +extern int fl_temp_cnt; +extern int fl_temp_cnt2; +extern int cur_vel_sum; + +// find home +extern int CUR_VELOCITY_OLD; +extern int cnt_findhome; +extern int cnt_vel_findhome; +extern int FINDHOME_VELOCITY; +extern int FINDHOME_VELOCITY_OLD; +extern int FINDHOME_POSITION; +extern int FINDHOME_POSITION_OLD; + +// valve gain +extern int check_vel_pos_init; +extern int check_vel_pos_fin; +extern int check_vel_pos_interv; +extern int valve_gain_repeat_cnt; +extern double VALVE_VOLTAGE; + +extern double freq_fric_tune; + +extern bool FLAG_VALVE_OUTPUT_CALIB; \ No newline at end of file