Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2
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.
Diff: main.cpp
- Revision:
- 50:3c630b5eba9f
- Parent:
- 49:e7bcfc244d40
- Child:
- 51:b46bed7fec80
--- a/main.cpp Wed Feb 05 08:35:58 2020 +0000
+++ b/main.cpp Thu Feb 13 05:16:15 2020 +0000
@@ -109,7 +109,7 @@
MODE_NO_ACT = 0, //0
MODE_VALVE_POSITION_CONTROL, //1
MODE_JOINT_CONTROL, //2
-
+
MODE_VALVE_OPEN_LOOP, //3
MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION, //4
MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING, //5
@@ -179,7 +179,7 @@
TIM3->CR1 ^= TIM_CR1_UDIS;
make_delay();
- // TMR5 init
+ // TMR2 init
Init_TMR2();
TIM2->CR1 ^= TIM_CR1_UDIS;
make_delay();
@@ -220,17 +220,14 @@
*************************************/
while(1) {
if(timer_while==1000) {
- //pc.printf("cmd %d\n ");
//i2c
read_field(i2c_slave_addr1);
if(DIR_VALVE_ENC < 0) value = 1023 - value;
-
// if(LED==1) {
// LED=0;
// } else
// LED = 1;
timer_while = 0;
-
}
timer_while ++;
}
@@ -244,15 +241,15 @@
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) {
- if(JOINT_VEL[i+1] == JOINT_VEL[i]){
+ if(JOINT_VEL[i+1] == JOINT_VEL[i]) {
Ref_Valve_Pos_FF = DDV_CENTER;
- }else{
+ } else {
Ref_Valve_Pos_FF = ((float) 10/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER;
}
} else {
- if(JOINT_VEL[i+1] == JOINT_VEL[i-1]){
- Ref_Valve_Pos_FF = DDV_CENTER;
- }else{
+ if(JOINT_VEL[i+1] == JOINT_VEL[i-1]) {
+ Ref_Valve_Pos_FF = DDV_CENTER;
+ } else {
Ref_Valve_Pos_FF = ((float) 10*(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 + (float) (10*ID_index_array[i-1]);
}
}
@@ -267,7 +264,7 @@
Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER);
return Ref_Valve_Pos_FF;
-
+
}
@@ -290,36 +287,6 @@
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_POS_VS_PWM[0])
-// {
-// if(REF_VALVE_POS <= VALVE_POS_VS_PWM[1]) {
-// VALVE_PWM_RAW_FF = (float) 3000.0 / (float) (VALVE_POS_VS_PWM[1] - VALVE_POS_VS_PWM[0]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[0]);
-// }else if(REF_VALVE_POS <= VALVE_POS_VS_PWM[3]) {
-// VALVE_PWM_RAW_FF = (float) 3000.0 / (float) (VALVE_POS_VS_PWM[3] - VALVE_POS_VS_PWM[1]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[1]) + 3000.0* (float) ID_index_array[1];
-// }else if(REF_VALVE_POS <= VALVE_POS_VS_PWM[5]) {
-// VALVE_PWM_RAW_FF = (float) 3000.0 / (float) (VALVE_POS_VS_PWM[5] - VALVE_POS_VS_PWM[3]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[3]) + 3000.0* (float) ID_index_array[3];
-// }else if(REF_VALVE_POS <= VALVE_POS_VS_PWM[7]) {
-// VALVE_PWM_RAW_FF = (float) 3000.0 / (float) (VALVE_POS_VS_PWM[7] - VALVE_POS_VS_PWM[5]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[5]) + 3000.0* (float) ID_index_array[5];
-// }else
-// VALVE_PWM_RAW_FF = 12000.0;
-// }
-// else
-// {
-// if(REF_VALVE_POS >= VALVE_POS_VS_PWM[2]) {
-// VALVE_PWM_RAW_FF = (float) 3000.0 / (float) (VALVE_POS_VS_PWM[0] - VALVE_POS_VS_PWM[2]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[2]) + 3000.0* (float) ID_index_array[2];
-// }else if(REF_VALVE_POS >= VALVE_POS_VS_PWM[4]) {
-// VALVE_PWM_RAW_FF = (float) 3000.0 / (float) (VALVE_POS_VS_PWM[2] - VALVE_POS_VS_PWM[4]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[4]) + 3000.0* (float) ID_index_array[4];
-// }else if(REF_VALVE_POS >= VALVE_POS_VS_PWM[6]) {
-// VALVE_PWM_RAW_FF = (float) 3000.0 / (float) (VALVE_POS_VS_PWM[4] - VALVE_POS_VS_PWM[6]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[6]) + 3000.0* (float) ID_index_array[6];
-// }else if(REF_VALVE_POS >= VALVE_POS_VS_PWM[8]) {
-// VALVE_PWM_RAW_FF = (float) 3000.0 / (float) (VALVE_POS_VS_PWM[6] - VALVE_POS_VS_PWM[8]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[8]) + 3000.0* (float) ID_index_array[8];
-// }else
-// VALVE_PWM_RAW_FF = -12000.0;
-// }
-
for(i=0; i<24; 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) {
@@ -330,11 +297,7 @@
break;
}
}
- if(vel.sen>=0.0f)
- V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
- else
- V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB; // - (float) FLOWFORCE_GAIN * 0.01f * vel.sen / 30.0f;
- //V_out = VALVE_PWM_RAW_FB;
+ V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
}
#define LT_MAX_IDX 57
@@ -411,7 +374,7 @@
float pres_A_new = ((float)ADC1->DR);
pres_A.sen = pres_A.sen*(1.0f-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A);
torq.sen = - (pres_A.sen-2048.0f); //pulse -2047~2047
-
+
//Pressure sensor 1B
//float alpha_update_pres_B = 1.0f/(1.0f+(FREQ_TMR4/2.0f)/(2.0f*3.14f*1000.0f));
@@ -426,7 +389,7 @@
// a1=ADC2->DR;
//int raw_cur = ADC3->DR;
//while((ADC3->SR & 0b10));
- float alpha_update_cur = 1.0f/(1.0f+(FREQ_TMR4/2.0f)/(2.0f*3.14f*1000.0f)); // f_cutoff : 500Hz
+ float alpha_update_cur = 1.0f/(1.0f+(FREQ_TMR4/2.0f)/(2.0f*3.14f*200.0f)); // f_cutoff : 500Hz
float cur_new = ((float)ADC3->DR-2048.0f)*20.0f/4096.0f; // unit : mA
cur.sen=cur.sen*(1.0f-alpha_update_cur)+cur_new*(alpha_update_cur);
//cur.sen = raw_cur;
@@ -453,31 +416,28 @@
{
if (TIM3->SR & TIM_SR_UIF ) {
ENC_UPDATE();
-
- if(MODE_POS_FT_TRANS == 1){
+
+ if(MODE_POS_FT_TRANS == 1) {
alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f;
cnt_trans++;
torq.err_sum = 0;
if((float)cnt_trans * DT_TMR3 > 3.0f)
MODE_POS_FT_TRANS = 2;
- }
- else if(MODE_POS_FT_TRANS == 3){
+ } else if(MODE_POS_FT_TRANS == 3) {
alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f;
cnt_trans++;
torq.err_sum = 0;
if((float) cnt_trans * DT_TMR3 > 3.0f )
MODE_POS_FT_TRANS = 0;
- }
- else if(MODE_POS_FT_TRANS == 2){
+ } else if(MODE_POS_FT_TRANS == 2) {
alpha_trans = 1.0;
cnt_trans = 0;
- }
- else{
+ } else {
alpha_trans = 0.0;
cnt_trans = 0;
}
-
+
// CONTROL LOOP ------------------------------------------------------------
@@ -493,34 +453,34 @@
}
case MODE_JOINT_CONTROL: {
-
+
float VALVE_POS_RAW_FORCE_FB = 0.0f;
-
+
pos.err = pos.ref - pos.sen; //[pulse]
vel.err = vel.ref - vel.sen; //[pulse/s]
double torq_ref = 0.0f;
torq_ref = torq.ref + (K_SPRING * pos.err * 0.01f + D_DAMPER * vel.err * 0.0001f) / ENC_PULSE_PER_POSITION; //[N]
-
+
// torque feedback
torq.err = torq_ref - torq.sen; //[pulse]
torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[pulse]
if (torq.err_sum > 1000) torq.err_sum = 1000;
if (torq.err_sum<-1000) torq.err_sum = -1000;
-
+
VALVE_POS_RAW_FORCE_FB = alpha_trans*(((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) /(float) TORQUE_SENSOR_PULSE_PER_TORQUE * 0.01f
- + DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err /(float) ENC_PULSE_PER_POSITION + DDV_JOINT_POS_FF(vel.ref));
+ + DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err /(float) ENC_PULSE_PER_POSITION + DDV_JOINT_POS_FF(vel.ref));
if (VALVE_POS_RAW_FORCE_FB >= 0) {
valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_PLUS;
- } else{
+ } else {
valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_MINUS;
}
VALVE_POS_CONTROL(valve_pos.ref);
break;
}
-
+
case MODE_VALVE_OPEN_LOOP: {
V_out = (float) Vout.ref;
break;
@@ -538,8 +498,7 @@
pos.err_sum += pos.err;
if (pos.err_sum > 1000) pos.err_sum = 1000;
if (pos.err_sum<-1000) pos.err_sum = -1000;
- VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err + (float) I_GAIN_JOINT_POSITION * 0.01f * pos.err_sum + (float) D_GAIN_JOINT_POSITION * pos.err_diff;
- VALVE_POS_RAW_POS_FB = VALVE_POS_RAW_POS_FB * 0.01f;
+ VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err/(float) ENC_PULSE_PER_POSITION + (float) I_GAIN_JOINT_POSITION * 0.01f * pos.err_sum/(float) ENC_PULSE_PER_POSITION + (float) D_GAIN_JOINT_POSITION * pos.err_diff/(float) ENC_PULSE_PER_POSITION;
//Ref_Joint_Vel = Ref_Vel_Test;
@@ -574,272 +533,272 @@
break;
}
- case MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING: {
-
- break;
- }
-
- case MODE_JOINT_POSITION_PRES_CONTROL_PWM: {
- pos.err = pos.ref - (float) pos.sen;
- pos.err_diff = pos.err - pos.err_old;
- pos.err_old = pos.err;
- pos.err_sum += pos.err;
- if (pos.err_sum > 1000) pos.err_sum = 1000;
- if (pos.err_sum<-1000) pos.err_sum = -1000;
- VALVE_PWM_RAW_POS = ((float) P_GAIN_JOINT_POSITION * pos.err + (float) I_GAIN_JOINT_POSITION * pos.err_sum + (float) D_GAIN_JOINT_POSITION * pos.err_diff) * 0.01f;
-
- torq.err = torq.ref - torq.sen;
- torq.err_diff = torq.err - torq.err_old;
- torq.err_old = torq.err;
- torq.err_sum += torq.err;
- if (torq.err_sum > 1000) torq.err_sum = 1000;
- if (torq.err_sum<-1000) torq.err_sum = -1000;
- VALVE_PWM_RAW_TORQ = (float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * torq.err_diff;
-
- VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01f;
-
- V_out = VALVE_PWM_RAW_POS + (float) COMPLIANCE_GAIN * 0.01f * VALVE_PWM_RAW_TORQ;
+// case MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING: {
+//
+// break;
+// }
- CUR_FLOWRATE = (float) CUR_VELOCITY * 0.00009587f;
- CUR_FLOWRATE = CUR_FLOWRATE * 0.5757f; // 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.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - 0.0f) / (VALVE_GAIN_LPM_PER_V[0]*1.0f - 0.0f) + 0.0f;
- else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[0]*1.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[2]*2.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[0]*1.0f) / (VALVE_GAIN_LPM_PER_V[2]*2.0f - VALVE_GAIN_LPM_PER_V[0]*1.) + 1.0f;
- else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[2]*2.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[4]*3.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[2]*2.0f) / (VALVE_GAIN_LPM_PER_V[4]*3.0f - VALVE_GAIN_LPM_PER_V[2]*2.) + 2.0f;
- else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[4]*3.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[6]*4.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[4]*3.0f) / (VALVE_GAIN_LPM_PER_V[6]*4.0f - VALVE_GAIN_LPM_PER_V[4]*3.) + 3.0f;
- else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[6]*4.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[8]*5.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[6]*4.0f) / (VALVE_GAIN_LPM_PER_V[8]*5.0f - VALVE_GAIN_LPM_PER_V[6]*4.) + 4.0f;
- else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[8]*5.0f)) VALVE_FF_VOLTAGE = 5.0f;
- else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) && CUR_FLOWRATE < 0.0f) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) / (0.0f - VALVE_GAIN_LPM_PER_V[1]*(-1.)) - 1.0f;
- else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[1]*(-1.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) / ((VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) - VALVE_GAIN_LPM_PER_V[3]*(-2.)) - 2.0f;
- else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[3]*(-2.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) / ((VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) - VALVE_GAIN_LPM_PER_V[5]*(-3.)) - 3.0f;
- else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[5]*(-3.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) / ((VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) - VALVE_GAIN_LPM_PER_V[7]*(-4.)) - 4.0f;
- else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[9]*(-5.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[7]*(-4.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[9]*(-5.0f)) / ((VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) - VALVE_GAIN_LPM_PER_V[9]*(-5.)) - 5.0f;
- else if (CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[9]*(-5.0f))) VALVE_FF_VOLTAGE = -5;
- else VALVE_FF_VOLTAGE = 0;
- } else {
- if (CUR_FLOWRATE >= 0 && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[0]*1.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - 0.0f) / (VALVE_GAIN_LPM_PER_V[0]*1.0f - 0.0f) + 0.0f;
- else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[1]*1.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[2]*2.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[0]*1.0f) / (VALVE_GAIN_LPM_PER_V[2]*2.0f - VALVE_GAIN_LPM_PER_V[0]*1.0f) + 1.0f;
- else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[3]*2.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[4]*3.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[2]*2.0f) / (VALVE_GAIN_LPM_PER_V[4]*3.0f - VALVE_GAIN_LPM_PER_V[2]*2.0f) + 2.0f;
- else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[5]*3.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[6]*4.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[4]*3.0f) / (VALVE_GAIN_LPM_PER_V[6]*4.0f - VALVE_GAIN_LPM_PER_V[4]*3.0f) + 3.0f;
- else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[7]*4.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[8]*5.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[6]*4.0f) / (VALVE_GAIN_LPM_PER_V[8]*5.0f - VALVE_GAIN_LPM_PER_V[6]*4.0f) + 4.0f;
- else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[9]*5.0f)) VALVE_FF_VOLTAGE = 5.0f;
- else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[0]*(-1.0f)) && CUR_FLOWRATE < 0.0f) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) / (0.0f - VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) - 1.0f;
- else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[2]*(-2.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[1]*(-1.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) / ((VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) - VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) - 2.0f;
- else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[4]*(-3.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[3]*(-2.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) / ((VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) - VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) - 3.0f;
- else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[6]*(-4.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[5]*(-3.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) / ((VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) - VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) - 4.0f;
- else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[8]*(-5.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[7]*(-4.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[9]*(-5.0f)) / ((VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) - VALVE_GAIN_LPM_PER_V[9]*(-5.0f)) - 5.0f;
- else if (CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[8]*(-5.0f))) VALVE_FF_VOLTAGE = -5;
- else VALVE_FF_VOLTAGE = 0;
- }
- // VALVE_FF_VOLTAGE = CUR_FLOWRATE * 0.5f;
+// case MODE_JOINT_POSITION_PRES_CONTROL_PWM: {
+// pos.err = pos.ref - (float) pos.sen;
+// pos.err_diff = pos.err - pos.err_old;
+// pos.err_old = pos.err;
+// pos.err_sum += pos.err;
+// if (pos.err_sum > 1000) pos.err_sum = 1000;
+// if (pos.err_sum<-1000) pos.err_sum = -1000;
+// VALVE_PWM_RAW_POS = ((float) P_GAIN_JOINT_POSITION * pos.err + (float) I_GAIN_JOINT_POSITION * pos.err_sum + (float) D_GAIN_JOINT_POSITION * pos.err_diff)/(float) ENC_PULSE_PER_POSITION;
+//
+// torq.err = torq.ref - torq.sen;
+// torq.err_diff = torq.err - torq.err_old;
+// torq.err_old = torq.err;
+// torq.err_sum += torq.err;
+// if (torq.err_sum > 1000) torq.err_sum = 1000;
+// if (torq.err_sum<-1000) torq.err_sum = -1000;
+// VALVE_PWM_RAW_TORQ = (float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * torq.err_diff;
+//
+// VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01f;
+//
+// V_out = VALVE_PWM_RAW_POS + (float) COMPLIANCE_GAIN * 0.01f * VALVE_PWM_RAW_TORQ;
+//
+// CUR_FLOWRATE = (float) CUR_VELOCITY * 0.00009587f;
+// CUR_FLOWRATE = CUR_FLOWRATE * 0.5757f; // 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.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - 0.0f) / (VALVE_GAIN_LPM_PER_V[0]*1.0f - 0.0f) + 0.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[0]*1.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[2]*2.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[0]*1.0f) / (VALVE_GAIN_LPM_PER_V[2]*2.0f - VALVE_GAIN_LPM_PER_V[0]*1.) + 1.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[2]*2.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[4]*3.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[2]*2.0f) / (VALVE_GAIN_LPM_PER_V[4]*3.0f - VALVE_GAIN_LPM_PER_V[2]*2.) + 2.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[4]*3.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[6]*4.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[4]*3.0f) / (VALVE_GAIN_LPM_PER_V[6]*4.0f - VALVE_GAIN_LPM_PER_V[4]*3.) + 3.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[6]*4.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[8]*5.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[6]*4.0f) / (VALVE_GAIN_LPM_PER_V[8]*5.0f - VALVE_GAIN_LPM_PER_V[6]*4.) + 4.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[8]*5.0f)) VALVE_FF_VOLTAGE = 5.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) && CUR_FLOWRATE < 0.0f) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) / (0.0f - VALVE_GAIN_LPM_PER_V[1]*(-1.)) - 1.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[1]*(-1.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) / ((VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) - VALVE_GAIN_LPM_PER_V[3]*(-2.)) - 2.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[3]*(-2.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) / ((VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) - VALVE_GAIN_LPM_PER_V[5]*(-3.)) - 3.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[5]*(-3.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) / ((VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) - VALVE_GAIN_LPM_PER_V[7]*(-4.)) - 4.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[9]*(-5.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[7]*(-4.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[9]*(-5.0f)) / ((VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) - VALVE_GAIN_LPM_PER_V[9]*(-5.)) - 5.0f;
+// else if (CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[9]*(-5.0f))) VALVE_FF_VOLTAGE = -5;
+// else VALVE_FF_VOLTAGE = 0;
+// } else {
+// if (CUR_FLOWRATE >= 0 && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[0]*1.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - 0.0f) / (VALVE_GAIN_LPM_PER_V[0]*1.0f - 0.0f) + 0.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[1]*1.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[2]*2.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[0]*1.0f) / (VALVE_GAIN_LPM_PER_V[2]*2.0f - VALVE_GAIN_LPM_PER_V[0]*1.0f) + 1.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[3]*2.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[4]*3.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[2]*2.0f) / (VALVE_GAIN_LPM_PER_V[4]*3.0f - VALVE_GAIN_LPM_PER_V[2]*2.0f) + 2.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[5]*3.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[6]*4.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[4]*3.0f) / (VALVE_GAIN_LPM_PER_V[6]*4.0f - VALVE_GAIN_LPM_PER_V[4]*3.0f) + 3.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[7]*4.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[8]*5.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[6]*4.0f) / (VALVE_GAIN_LPM_PER_V[8]*5.0f - VALVE_GAIN_LPM_PER_V[6]*4.0f) + 4.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[9]*5.0f)) VALVE_FF_VOLTAGE = 5.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[0]*(-1.0f)) && CUR_FLOWRATE < 0.0f) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) / (0.0f - VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) - 1.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[2]*(-2.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[1]*(-1.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) / ((VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) - VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) - 2.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[4]*(-3.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[3]*(-2.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) / ((VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) - VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) - 3.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[6]*(-4.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[5]*(-3.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) / ((VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) - VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) - 4.0f;
+// else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[8]*(-5.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[7]*(-4.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[9]*(-5.0f)) / ((VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) - VALVE_GAIN_LPM_PER_V[9]*(-5.0f)) - 5.0f;
+// else if (CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[8]*(-5.0f))) VALVE_FF_VOLTAGE = -5;
+// else VALVE_FF_VOLTAGE = 0;
+// }
+// // VALVE_FF_VOLTAGE = CUR_FLOWRATE * 0.5f;
+//
+// if (CUR_FLOWRATE >= 0) VALVE_FF_VOLTAGE = (float) VELOCITY_COMP_GAIN * 0.001f * VALVE_FF_VOLTAGE * sqrt((float) PRES_SUPPLY - CUR_PRES_A_BAR) * 0.0707f; // 0.0707 = 1/sqrt(200.))
+// else if (CUR_FLOWRATE < 0) VALVE_FF_VOLTAGE = (float) VELOCITY_COMP_GAIN * 0.001f * VALVE_FF_VOLTAGE * sqrt((float) PRES_SUPPLY - CUR_PRES_B_BAR) * 0.0707f;
+//
+// V_out = V_out + VALVE_FF_VOLTAGE;
+// break;
+// }
- if (CUR_FLOWRATE >= 0) VALVE_FF_VOLTAGE = (float) VELOCITY_COMP_GAIN * 0.001f * VALVE_FF_VOLTAGE * sqrt((float) PRES_SUPPLY - CUR_PRES_A_BAR) * 0.0707f; // 0.0707 = 1/sqrt(200.))
- else if (CUR_FLOWRATE < 0) VALVE_FF_VOLTAGE = (float) VELOCITY_COMP_GAIN * 0.001f * VALVE_FF_VOLTAGE * sqrt((float) PRES_SUPPLY - CUR_PRES_B_BAR) * 0.0707f;
-
- V_out = V_out + VALVE_FF_VOLTAGE;
- break;
- }
-
- case MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION: {
-
- pos.err = pos.ref - (float) pos.sen;
- pos.err_diff = pos.err - pos.err_old;
- pos.err_old = pos.err;
- pos.err_sum += pos.err;
- if (pos.err_sum > 1000) pos.err_sum = 1000;
- if (pos.err_sum<-1000) pos.err_sum = -1000;
- VALVE_PWM_RAW_POS = ((float) P_GAIN_JOINT_POSITION * pos.err + (float) I_GAIN_JOINT_POSITION * pos.err_sum + (float) D_GAIN_JOINT_POSITION * pos.err_diff) * 0.01f;
+// case MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION: {
+//
+// pos.err = pos.ref - (float) pos.sen;
+// pos.err_diff = pos.err - pos.err_old;
+// pos.err_old = pos.err;
+// pos.err_sum += pos.err;
+// if (pos.err_sum > 1000) pos.err_sum = 1000;
+// if (pos.err_sum<-1000) pos.err_sum = -1000;
+// VALVE_PWM_RAW_POS = ((float) P_GAIN_JOINT_POSITION * pos.err + (float) I_GAIN_JOINT_POSITION * pos.err_sum + (float) D_GAIN_JOINT_POSITION * pos.err_diff) * 0.01f;
+//
+// torq.err = torq.ref - torq.sen;
+// torq.err_diff = torq.err - torq.err_old;
+// torq.err_old = torq.err;
+// torq.err_sum += torq.err;
+// if (torq.err_sum > 1000) torq.err_sum = 1000;
+// if (torq.err_sum<-1000) torq.err_sum = -1000;
+// VALVE_PWM_RAW_TORQ = (float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * torq.err_diff;
+//
+// valve_pos.ref = VALVE_PWM_RAW_POS + VALVE_PWM_RAW_TORQ;
+// VALVE_POS_CONTROL(valve_pos.ref);
+//
+// break;
+// }
- torq.err = torq.ref - torq.sen;
- torq.err_diff = torq.err - torq.err_old;
- torq.err_old = torq.err;
- torq.err_sum += torq.err;
- if (torq.err_sum > 1000) torq.err_sum = 1000;
- if (torq.err_sum<-1000) torq.err_sum = -1000;
- VALVE_PWM_RAW_TORQ = (float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * torq.err_diff;
-
- valve_pos.ref = VALVE_PWM_RAW_POS + VALVE_PWM_RAW_TORQ;
- VALVE_POS_CONTROL(valve_pos.ref);
-
- break;
- }
-
- case MODE_VALVE_POSITION_PRES_CONTROL_LEARNING: {
-
- break;
- }
+// case MODE_VALVE_POSITION_PRES_CONTROL_LEARNING: {
+//
+// break;
+// }
- case MODE_TEST_CURRENT_CONTROL: {
- if (TMR3_COUNT_IREF == TMR_FREQ_5k) {
- TMR3_COUNT_IREF = 0;
- }
- TMR3_COUNT_IREF++;
-
- // Set Current Reference
- float TMR3_CNT_MAX = (float)TMR_FREQ_5k/2.0f;
- float I_REF_MID = 0.0f;
- if (TMR3_COUNT_IREF < TMR3_CNT_MAX) {
- I_REF = I_REF_MID + 1.0f;
- } else {
- I_REF = I_REF_MID - 1.0f;
- }
-// float T = 1.0; // wave period
-// I_REF = (5. * sin(2. * 3.1415 * (float) TMR3_COUNT_IREF / (float)TMR_FREQ_5k/ T));
-// I_REF = (2.0 * sin(2. * 2. * 3.14 * (float) TMR3_COUNT_IREF / 5000.)+(2.0 * sin(2. * 1. * 3.14 * (float)TMR3_COUNT_IREF/ 5000.))+(2.0 * sin(2. * 5. * 3.14 * (float)TMR3_COUNT_IREF/ 5000.))+(2.0 * sin(2. * 10. * 3.14 * (float)TMR3_COUNT_IREF/ 5000.)));
+// case MODE_TEST_CURRENT_CONTROL: {
+// if (TMR3_COUNT_IREF == TMR_FREQ_5k) {
+// TMR3_COUNT_IREF = 0;
+// }
+// TMR3_COUNT_IREF++;
+//
+// // Set Current Reference
+// float TMR3_CNT_MAX = (float)TMR_FREQ_5k/2.0f;
+// float I_REF_MID = 0.0f;
+// if (TMR3_COUNT_IREF < TMR3_CNT_MAX) {
+// I_REF = I_REF_MID + 1.0f;
+// } else {
+// I_REF = I_REF_MID - 1.0f;
+// }
+//// float T = 1.0; // wave period
+//// I_REF = (5. * sin(2. * 3.1415 * (float) TMR3_COUNT_IREF / (float)TMR_FREQ_5k/ T));
+//// I_REF = (2.0 * sin(2. * 2. * 3.14 * (float) TMR3_COUNT_IREF / 5000.)+(2.0 * sin(2. * 1. * 3.14 * (float)TMR3_COUNT_IREF/ 5000.))+(2.0 * sin(2. * 5. * 3.14 * (float)TMR3_COUNT_IREF/ 5000.))+(2.0 * sin(2. * 10. * 3.14 * (float)TMR3_COUNT_IREF/ 5000.)));
+//
+// if (TMR3_COUNT_IREF % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
+// //CAN_TX_PRES((int16_t)(I_REF*1000.0), (int16_t) (CUR_CURRENT*1000.0)); // to check the datas
+// }
+// break;
+// }
- if (TMR3_COUNT_IREF % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
- //CAN_TX_PRES((int16_t)(I_REF*1000.0), (int16_t) (CUR_CURRENT*1000.0)); // to check the datas
- }
- break;
- }
-
- case MODE_TEST_PWM_CONTROL: {
- if (TMR3_COUNT_IREF == TMR_FREQ_5k) {
- TMR3_COUNT_IREF = 0;
- }
- TMR3_COUNT_IREF++;
-
- // Set PWM reference
- float TMR3_CNT_MAX = (float)TMR_FREQ_5k/2.0f;
- //float I_REF_MID = 0.0f;
- if (TMR3_COUNT_IREF < TMR3_CNT_MAX) {
- CUR_PWM = 1000;
- } else {
- CUR_PWM = -1000;
- }
-
- if (TMR3_COUNT_IREF % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
- //CAN_TX_PRES((int16_t)(u_CUR[0]*1000.0f), (int16_t) (CUR_CURRENT*1000.0f)); // to check the datas
- }
- break;
- }
+// case MODE_TEST_PWM_CONTROL: {
+// if (TMR3_COUNT_IREF == TMR_FREQ_5k) {
+// TMR3_COUNT_IREF = 0;
+// }
+// TMR3_COUNT_IREF++;
+//
+// // Set PWM reference
+// float TMR3_CNT_MAX = (float)TMR_FREQ_5k/2.0f;
+// //float I_REF_MID = 0.0f;
+// if (TMR3_COUNT_IREF < TMR3_CNT_MAX) {
+// CUR_PWM = 1000;
+// } else {
+// CUR_PWM = -1000;
+// }
+//
+// if (TMR3_COUNT_IREF % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
+// //CAN_TX_PRES((int16_t)(u_CUR[0]*1000.0f), (int16_t) (CUR_CURRENT*1000.0f)); // to check the datas
+// }
+// break;
+// }
- case MODE_CURRENT_CONTROL: {
-
- cur.ref = cur.ref; // Unit : mA
- CurrentControl();
- break;
- }
-
- case MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT: {
- float I_REF_POS_FB = 0.0f; // I_REF by Position Feedback
- float I_REF_POS_FF = 0.0f; // I_REF by Position Feedforward
- float I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback
- float I_REF_FORCE_FF = 0.0f; // I_REF by Force Feedforward
-
- // feedback input for position control
- pos.err = pos.ref - pos.sen;
- float alpha_update_vel = 1.0f/(1.0f+(float)FREQ_TMR4/(2.0f*3.1415f*50.0f)); // f_cutoff : 50Hz
- float err_diff = (pos.err - pos.err_old)*(float)FREQ_5k;
- pos.err_diff = (1.0f-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff;
- pos.err_old = pos.err;
- I_REF_POS_FB = 0.001f*((float)P_GAIN_JOINT_POSITION * pos.err + (float)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1f);
-
- // feedforward input for position control
- float Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
- float K_ff = 1.3f;
- float K_v = 0.0f;
- if(Vel_Act_Ref > 0) K_v = 1.0f/100.0f; // open, tuning. (deg/s >> mA)
- if(Vel_Act_Ref < 0) K_v = 1.0f/100.0f; // close, tuning. (deg/s >> mA)
- I_REF_POS_FF = K_ff*K_v*Vel_Act_Ref;
+// case MODE_CURRENT_CONTROL: {
+//
+// cur.ref = cur.ref; // Unit : mA
+// CurrentControl();
+// break;
+// }
- // feedback input for position control
- I_REF_FORCE_FB = 0.0f;
-
- // feedforward input for position control
- I_REF_FORCE_FF = 0.0f;
-
- cur.ref = I_REF_POS_FF + I_REF_POS_FB + I_REF_FORCE_FB + I_REF_FORCE_FF;
-
- CurrentControl();
-
- break;
- }
-
- case MODE_JOINT_POSITION_PRES_CONTROL_CURRENT: {
- //float T_REF = 0.0; // Torque Reference
- float I_REF_FORCE_FB = 0.; // I_REF by Force Feedback
- float I_REF_VC = 0.; // I_REF for velocity compensation
-
- // feedback input for position control
- //float Joint_Pos_Err = 34.0f-(float) pos.sen/(float)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
- //float Joint_Vel_Err = 0.0f-(float) vel.sen/(float)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
- //float K_spring = 0.7f;
- //float D_damper = 0.02f;
-// T_REF = K_spring * pos.err + D_damper * Joint_Vel_Err; // unit : Nm
-
- // torque feedback
- torq.err = torq.ref - torq.sen;
- // torq.err_diff = torq.err - torq.err_old;
- // torq.err_old = torq.err;
- torq.err_sum = torq.err_sum + torq.err/(float)TMR_FREQ_5k;
- I_REF_FORCE_FB = 0.001f*((float)P_GAIN_JOINT_TORQUE * torq.err + (float)I_GAIN_JOINT_TORQUE * torq.err_sum);
+// case MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT: {
+// float I_REF_POS_FB = 0.0f; // I_REF by Position Feedback
+// float I_REF_POS_FF = 0.0f; // I_REF by Position Feedforward
+// float I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback
+// float I_REF_FORCE_FF = 0.0f; // I_REF by Force Feedforward
+//
+// // feedback input for position control
+// pos.err = pos.ref - pos.sen;
+// float alpha_update_vel = 1.0f/(1.0f+(float)FREQ_TMR4/(2.0f*3.1415f*50.0f)); // f_cutoff : 50Hz
+// float err_diff = (pos.err - pos.err_old)*(float)FREQ_5k;
+// pos.err_diff = (1.0f-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff;
+// pos.err_old = pos.err;
+// I_REF_POS_FB = 0.001f*((float)P_GAIN_JOINT_POSITION * pos.err + (float)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1f);
+//
+// // feedforward input for position control
+// float Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
+// float K_ff = 1.3f;
+// float K_v = 0.0f;
+// if(Vel_Act_Ref > 0) K_v = 1.0f/100.0f; // open, tuning. (deg/s >> mA)
+// if(Vel_Act_Ref < 0) K_v = 1.0f/100.0f; // close, tuning. (deg/s >> mA)
+// I_REF_POS_FF = K_ff*K_v*Vel_Act_Ref;
+//
+// // feedback input for position control
+// I_REF_FORCE_FB = 0.0f;
+//
+// // feedforward input for position control
+// I_REF_FORCE_FF = 0.0f;
+//
+// cur.ref = I_REF_POS_FF + I_REF_POS_FB + I_REF_FORCE_FB + I_REF_FORCE_FF;
+//
+// CurrentControl();
+//
+// break;
+// }
- // velocity compensation for torque control
- float Joint_Vel_Act = vel.sen/(float)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
- float K_vc = 1.5f; // Velocity comp. gain
- float K_v = 0.0f; // Valve gain
- if(Joint_Vel_Act > 0) K_v = 1.0f/100.0f; // open, tuning
- if(Joint_Vel_Act < 0) K_v = 1.0f/100.0f; // close, tuning
- I_REF_VC = K_vc*K_v*Joint_Vel_Act;
-
- cur.ref = I_REF_VC + I_REF_FORCE_FB;
- // cur.ref = I_REF_FORCE_FB;
-
- float I_MAX = 10.0f; // Maximum Current : 10mV
- float Ka = 1.0f/I_GAIN_JOINT_TORQUE;
- if(cur.ref > I_MAX) {
- float I_rem = cur.ref-I_MAX;
- I_rem = Ka*I_rem;
- cur.ref = I_MAX;
- torq.err_sum = torq.err_sum - I_rem/(float)TMR_FREQ_5k;
- } else if(cur.ref < -I_MAX) {
- float I_rem = cur.ref-(-I_MAX);
- I_rem = Ka*I_rem;
- cur.ref = -I_MAX;
- torq.err_sum = torq.err_sum - I_rem/(float)TMR_FREQ_5k;
- }
-
- CurrentControl();
-
-
- /*
- float I_REF_POS_FB = 0.0f; // I_REF by Position Feedback
- float I_REF_POS_FF = 0.0f; // I_REF by Position Feedforward
- float I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback
- float I_REF_FORCE_FF = 0.0f; // I_REF by Force Feedforward
-
- // feedback input for position control
- pos.err = pos.ref - pos.sen;
- float alpha_update_vel = 1.0f/(1.0f+(float)FREQ_TMR4/(2.0f*3.1415f*50.0f)); // f_cutoff : 50Hz
- float err_diff = (pos.err - pos.err_old)*(float)FREQ_5k;
- pos.err_diff = (1.0f-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff;
- pos.err_old = pos.err;
- I_REF_POS_FB = 0.001f*((float)P_GAIN_JOINT_POSITION * pos.err + (float)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1f);
-
- // feedforward input for position control
- float Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
- float K_ff = 1.3f;
- float K_v = 0.0f;
- if(Vel_Act_Ref > 0) K_v = 1.0f/100.0f; // open, tuning. (deg/s >> mA)
- if(Vel_Act_Ref < 0) K_v = 1.0f/100.0f; // close, tuning. (deg/s >> mA)
- I_REF_POS_FF = K_ff*K_v*Vel_Act_Ref;
-
- // feedback input for position control
- I_REF_FORCE_FB = 0.0f;
-
- // feedforward input for position control
- I_REF_FORCE_FF = 0.0f;
-
- cur.ref = I_REF_POS_FF + I_REF_POS_FB + I_REF_FORCE_FB + I_REF_FORCE_FF;
-
- CurrentControl();
- */
-
- break;
- }
+// case MODE_JOINT_POSITION_PRES_CONTROL_CURRENT: {
+// //float T_REF = 0.0; // Torque Reference
+// float I_REF_FORCE_FB = 0.; // I_REF by Force Feedback
+// float I_REF_VC = 0.; // I_REF for velocity compensation
+//
+// // feedback input for position control
+// //float Joint_Pos_Err = 34.0f-(float) pos.sen/(float)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
+// //float Joint_Vel_Err = 0.0f-(float) vel.sen/(float)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
+// //float K_spring = 0.7f;
+// //float D_damper = 0.02f;
+//// T_REF = K_spring * pos.err + D_damper * Joint_Vel_Err; // unit : Nm
+//
+// // torque feedback
+// torq.err = torq.ref - torq.sen;
+// // torq.err_diff = torq.err - torq.err_old;
+// // torq.err_old = torq.err;
+// torq.err_sum = torq.err_sum + torq.err/(float)TMR_FREQ_5k;
+// I_REF_FORCE_FB = 0.001f*((float)P_GAIN_JOINT_TORQUE * torq.err + (float)I_GAIN_JOINT_TORQUE * torq.err_sum);
+//
+// // velocity compensation for torque control
+// float Joint_Vel_Act = vel.sen/(float)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
+// float K_vc = 1.5f; // Velocity comp. gain
+// float K_v = 0.0f; // Valve gain
+// if(Joint_Vel_Act > 0) K_v = 1.0f/100.0f; // open, tuning
+// if(Joint_Vel_Act < 0) K_v = 1.0f/100.0f; // close, tuning
+// I_REF_VC = K_vc*K_v*Joint_Vel_Act;
+//
+// cur.ref = I_REF_VC + I_REF_FORCE_FB;
+// // cur.ref = I_REF_FORCE_FB;
+//
+// float I_MAX = 10.0f; // Maximum Current : 10mV
+// float Ka = 1.0f/I_GAIN_JOINT_TORQUE;
+// if(cur.ref > I_MAX) {
+// float I_rem = cur.ref-I_MAX;
+// I_rem = Ka*I_rem;
+// cur.ref = I_MAX;
+// torq.err_sum = torq.err_sum - I_rem/(float)TMR_FREQ_5k;
+// } else if(cur.ref < -I_MAX) {
+// float I_rem = cur.ref-(-I_MAX);
+// I_rem = Ka*I_rem;
+// cur.ref = -I_MAX;
+// torq.err_sum = torq.err_sum - I_rem/(float)TMR_FREQ_5k;
+// }
+//
+// CurrentControl();
+//
+//
+// /*
+// float I_REF_POS_FB = 0.0f; // I_REF by Position Feedback
+// float I_REF_POS_FF = 0.0f; // I_REF by Position Feedforward
+// float I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback
+// float I_REF_FORCE_FF = 0.0f; // I_REF by Force Feedforward
+//
+// // feedback input for position control
+// pos.err = pos.ref - pos.sen;
+// float alpha_update_vel = 1.0f/(1.0f+(float)FREQ_TMR4/(2.0f*3.1415f*50.0f)); // f_cutoff : 50Hz
+// float err_diff = (pos.err - pos.err_old)*(float)FREQ_5k;
+// pos.err_diff = (1.0f-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff;
+// pos.err_old = pos.err;
+// I_REF_POS_FB = 0.001f*((float)P_GAIN_JOINT_POSITION * pos.err + (float)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1f);
+//
+// // feedforward input for position control
+// float Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
+// float K_ff = 1.3f;
+// float K_v = 0.0f;
+// if(Vel_Act_Ref > 0) K_v = 1.0f/100.0f; // open, tuning. (deg/s >> mA)
+// if(Vel_Act_Ref < 0) K_v = 1.0f/100.0f; // close, tuning. (deg/s >> mA)
+// I_REF_POS_FF = K_ff*K_v*Vel_Act_Ref;
+//
+// // feedback input for position control
+// I_REF_FORCE_FB = 0.0f;
+//
+// // feedforward input for position control
+// I_REF_FORCE_FF = 0.0f;
+//
+// cur.ref = I_REF_POS_FF + I_REF_POS_FB + I_REF_FORCE_FB + I_REF_FORCE_FF;
+//
+// CurrentControl();
+// */
+//
+// break;
+// }
case MODE_TORQUE_SENSOR_NULLING: {
// DAC Voltage reference set
@@ -854,7 +813,7 @@
if (TORQUE_VREF > 3.3f) TORQUE_VREF = 3.3f;
if (TORQUE_VREF < 0) TORQUE_VREF = 0;
-
+
//spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0));
dac_1 = TORQUE_VREF / 3.3f;
}
@@ -874,10 +833,10 @@
}
TMR3_COUNT_TORQUE_NULL++;
break;
-
-
-
- // // DAC Voltage reference set
+
+
+
+ // // DAC Voltage reference set
// if (TMR3_COUNT_PRES_NULL < TMR_FREQ_5k * 2) {
// CUR_PRES_A_sum += pres_A.sen;
// CUR_PRES_B_sum += pres_B.sen;
@@ -916,89 +875,89 @@
// }
// TMR3_COUNT_PRES_NULL++;
// break;
-
-
-
-
-
+
+
+
+
+
}
- case MODE_VALVE_NULLING_AND_DEADZONE_SETTING: {
- if (TMR3_COUNT_DEADZONE == 0) {
- if (pos_plus_end == pos_minus_end) need_enc_init = true;
- else temp_time = 0;
- }
- if (need_enc_init) {
- if (TMR3_COUNT_DEADZONE < (int) (0.5f * (float) TMR_FREQ_5k)) {
- V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
- pos_plus_end = pos.sen;
- } else if (TMR3_COUNT_DEADZONE < TMR_FREQ_5k) {
- V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
- pos_minus_end = pos.sen;
- } else if (TMR3_COUNT_DEADZONE == TMR_FREQ_5k) need_enc_init = false;
- temp_time = TMR_FREQ_5k;
- }
-
- if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + TMR_FREQ_5k)) {
- V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
- VALVE_CENTER = VALVE_DEADZONE_PLUS = VALVE_DEADZONE_MINUS = 0;
-
- } else if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + (int) (1.9f * (float) TMR_FREQ_5k))) {
- V_out = 0;
- CUR_VELOCITY_sum += CUR_VELOCITY;
- } else if (TMR3_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 (TMR3_COUNT_DEADZONE > (temp_time + 2 * TMR_FREQ_5k)) {
- if (TMR3_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 (TMR3_COUNT_DEADZONE % 20 != 0) {
- CUR_VELOCITY_sum += CUR_VELOCITY;
- } else {
- V_out -= 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) V_out;
- else VALVE_DEADZONE_PLUS = (int16_t) V_out;
- DZ_dir = -DZ_dir;
- DZ_temp_cnt = 0;
- DZ_temp_cnt2++;
- }
- } else {
- TMR3_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;
- }
- V_out = 0;
-
- ROM_RESET_DATA();
-
- //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;
- }
- }
- TMR3_COUNT_DEADZONE++;
- break;
- }
+// case MODE_VALVE_NULLING_AND_DEADZONE_SETTING: {
+// if (TMR3_COUNT_DEADZONE == 0) {
+// if (pos_plus_end == pos_minus_end) need_enc_init = true;
+// else temp_time = 0;
+// }
+// if (need_enc_init) {
+// if (TMR3_COUNT_DEADZONE < (int) (0.5f * (float) TMR_FREQ_5k)) {
+// V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
+// pos_plus_end = pos.sen;
+// } else if (TMR3_COUNT_DEADZONE < TMR_FREQ_5k) {
+// V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
+// pos_minus_end = pos.sen;
+// } else if (TMR3_COUNT_DEADZONE == TMR_FREQ_5k) need_enc_init = false;
+// temp_time = TMR_FREQ_5k;
+// }
+//
+// if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + TMR_FREQ_5k)) {
+// V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
+// VALVE_CENTER = VALVE_DEADZONE_PLUS = VALVE_DEADZONE_MINUS = 0;
+//
+// } else if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + (int) (1.9f * (float) TMR_FREQ_5k))) {
+// V_out = 0;
+// CUR_VELOCITY_sum += CUR_VELOCITY;
+// } else if (TMR3_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 (TMR3_COUNT_DEADZONE > (temp_time + 2 * TMR_FREQ_5k)) {
+// if (TMR3_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 (TMR3_COUNT_DEADZONE % 20 != 0) {
+// CUR_VELOCITY_sum += CUR_VELOCITY;
+// } else {
+// V_out -= 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) V_out;
+// else VALVE_DEADZONE_PLUS = (int16_t) V_out;
+// DZ_dir = -DZ_dir;
+// DZ_temp_cnt = 0;
+// DZ_temp_cnt2++;
+// }
+// } else {
+// TMR3_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;
+// }
+// V_out = 0;
+//
+// ROM_RESET_DATA();
+//
+// //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;
+// }
+// }
+// TMR3_COUNT_DEADZONE++;
+// break;
+// }
case MODE_FIND_HOME: {
if (FINDHOME_STAGE == FINDHOME_INIT) {
@@ -1031,8 +990,7 @@
else pos.ref_home_pos = pos.ref_home_pos - 2.0f;
pos.err = pos.ref_home_pos - pos.sen;
float VALVE_POS_RAW_POS_FB = 0.0f;
- VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err;
- VALVE_POS_RAW_POS_FB = VALVE_POS_RAW_POS_FB * 0.01f;
+ VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * pos.err/(float) ENC_PULSE_PER_POSITION * 0.01f;
valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
VALVE_POS_CONTROL(valve_pos.ref);
@@ -1062,8 +1020,7 @@
// input for position control
pos.err = pos.ref_home_pos - (float)pos.sen;
float VALVE_POS_RAW_POS_FB = 0.0f;
- VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err;
- VALVE_POS_RAW_POS_FB = VALVE_POS_RAW_POS_FB * 0.01f;
+ VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err/(float) ENC_PULSE_PER_POSITION;
valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
VALVE_POS_CONTROL(valve_pos.ref);
@@ -1102,77 +1059,77 @@
break;
}
- case MODE_VALVE_GAIN_SETTING: {
- if (TMR3_COUNT_FLOWRATE == 0) {
- if (pos_plus_end == pos_minus_end) need_enc_init = true;
- else {
- V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
- temp_time = (int) (0.5f * (float) TMR_FREQ_5k);
- }
- }
- if (need_enc_init) {
- if (TMR3_COUNT_FLOWRATE < (int) (0.5f * (float) TMR_FREQ_5k)) {
- V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
- pos_plus_end = pos.sen;
- } else if (TMR3_COUNT_FLOWRATE < TMR_FREQ_5k) {
- V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
- pos_minus_end = pos.sen;
- } else if (TMR3_COUNT_FLOWRATE == TMR_FREQ_5k) {
- need_enc_init = false;
- check_vel_pos_init = (int) (0.9f * (float) (pos_plus_end - pos_minus_end));
- check_vel_pos_fin = (int) (0.95f * (float) (pos_plus_end - pos_minus_end));
- check_vel_pos_interv = check_vel_pos_fin - check_vel_pos_init;
- }
- temp_time = TMR_FREQ_5k;
- }
- TMR3_COUNT_FLOWRATE++;
- if (TMR3_COUNT_FLOWRATE > temp_time) {
- if (flag_flowrate % 2 == 0) { // (+)
- VALVE_VOLTAGE = 1000.0f * (float) (flag_flowrate / 2 + 1);
- V_out = VALVE_VOLTAGE;
- if (pos.sen > (pos_minus_end + check_vel_pos_init) && pos.sen < (pos_minus_end + check_vel_pos_fin)) {
- fl_temp_cnt++;
- } else if (pos.sen >= (pos_minus_end + check_vel_pos_fin) && CUR_VELOCITY == 0) {
- VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873f * 0.5757f * (float) TMR_FREQ_5k / 10000.0 * (float) check_vel_pos_interv / (float) 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] = (float) TMR_FREQ_10k * (float) check_vel_pos_interv / (float) fl_temp_cnt / VALVE_VOLTAGE; // PULSE/sec
- fl_temp_cnt2++;
- }
- } else if (flag_flowrate % 2 == 1) { // (-)
- VALVE_VOLTAGE = -1. * (float) (flag_flowrate / 2 + 1);
- V_out = VALVE_VOLTAGE;
- if (pos.sen < (pos_plus_end - check_vel_pos_init) && pos.sen > (pos_plus_end - check_vel_pos_fin)) {
- fl_temp_cnt++;
- } else if (pos.sen <= (pos_plus_end - check_vel_pos_fin) && CUR_VELOCITY == 0) {
- VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873f * 0.5757f * (float) TMR_FREQ_5k / 10000.0f * (float) check_vel_pos_interv / (float) fl_temp_cnt / (-VALVE_VOLTAGE);
- // VALVE_GAIN_LPM_PER_V[flag_flowrate] = (float) TMR_FREQ_10k * (float) check_vel_pos_interv / (float) fl_temp_cnt / (-VALVE_VOLTAGE); // PULSE/sec
- fl_temp_cnt2++;
- }
- }
- if (fl_temp_cnt2 == 100) {
-
- ROM_RESET_DATA();
-
- //spi_eeprom_write(RID_VALVE_GAIN_PLUS_1 + flag_flowrate, (int16_t) (VALVE_GAIN_LPM_PER_V[flag_flowrate] * 100.0f));
- cur_vel_sum = 0;
- fl_temp_cnt = 0;
- fl_temp_cnt2 = 0;
- flag_flowrate++;
- }
- if (flag_flowrate == 10) {
- V_out = 0;
- flag_flowrate = 0;
- TMR3_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_VALVE_GAIN_SETTING: {
+// if (TMR3_COUNT_FLOWRATE == 0) {
+// if (pos_plus_end == pos_minus_end) need_enc_init = true;
+// else {
+// V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
+// temp_time = (int) (0.5f * (float) TMR_FREQ_5k);
+// }
+// }
+// if (need_enc_init) {
+// if (TMR3_COUNT_FLOWRATE < (int) (0.5f * (float) TMR_FREQ_5k)) {
+// V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
+// pos_plus_end = pos.sen;
+// } else if (TMR3_COUNT_FLOWRATE < TMR_FREQ_5k) {
+// V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
+// pos_minus_end = pos.sen;
+// } else if (TMR3_COUNT_FLOWRATE == TMR_FREQ_5k) {
+// need_enc_init = false;
+// check_vel_pos_init = (int) (0.9f * (float) (pos_plus_end - pos_minus_end));
+// check_vel_pos_fin = (int) (0.95f * (float) (pos_plus_end - pos_minus_end));
+// check_vel_pos_interv = check_vel_pos_fin - check_vel_pos_init;
+// }
+// temp_time = TMR_FREQ_5k;
+// }
+// TMR3_COUNT_FLOWRATE++;
+// if (TMR3_COUNT_FLOWRATE > temp_time) {
+// if (flag_flowrate % 2 == 0) { // (+)
+// VALVE_VOLTAGE = 1000.0f * (float) (flag_flowrate / 2 + 1);
+// V_out = VALVE_VOLTAGE;
+// if (pos.sen > (pos_minus_end + check_vel_pos_init) && pos.sen < (pos_minus_end + check_vel_pos_fin)) {
+// fl_temp_cnt++;
+// } else if (pos.sen >= (pos_minus_end + check_vel_pos_fin) && CUR_VELOCITY == 0) {
+// VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873f * 0.5757f * (float) TMR_FREQ_5k / 10000.0 * (float) check_vel_pos_interv / (float) 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] = (float) TMR_FREQ_10k * (float) check_vel_pos_interv / (float) fl_temp_cnt / VALVE_VOLTAGE; // PULSE/sec
+// fl_temp_cnt2++;
+// }
+// } else if (flag_flowrate % 2 == 1) { // (-)
+// VALVE_VOLTAGE = -1. * (float) (flag_flowrate / 2 + 1);
+// V_out = VALVE_VOLTAGE;
+// if (pos.sen < (pos_plus_end - check_vel_pos_init) && pos.sen > (pos_plus_end - check_vel_pos_fin)) {
+// fl_temp_cnt++;
+// } else if (pos.sen <= (pos_plus_end - check_vel_pos_fin) && CUR_VELOCITY == 0) {
+// VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873f * 0.5757f * (float) TMR_FREQ_5k / 10000.0f * (float) check_vel_pos_interv / (float) fl_temp_cnt / (-VALVE_VOLTAGE);
+// // VALVE_GAIN_LPM_PER_V[flag_flowrate] = (float) TMR_FREQ_10k * (float) check_vel_pos_interv / (float) fl_temp_cnt / (-VALVE_VOLTAGE); // PULSE/sec
+// fl_temp_cnt2++;
+// }
+// }
+// if (fl_temp_cnt2 == 100) {
+//
+// ROM_RESET_DATA();
+//
+// //spi_eeprom_write(RID_VALVE_GAIN_PLUS_1 + flag_flowrate, (int16_t) (VALVE_GAIN_LPM_PER_V[flag_flowrate] * 100.0f));
+// cur_vel_sum = 0;
+// fl_temp_cnt = 0;
+// fl_temp_cnt2 = 0;
+// flag_flowrate++;
+// }
+// if (flag_flowrate == 10) {
+// V_out = 0;
+// flag_flowrate = 0;
+// TMR3_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
@@ -1216,52 +1173,52 @@
break;
}
- case MODE_PRESSURE_SENSOR_CALIB: {
- if (TMR3_COUNT_PRES_CALIB < 2 * TMR_FREQ_5k) {
- V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
- if (TMR3_COUNT_PRES_CALIB >= TMR_FREQ_5k) {
- CUR_PRES_A_sum += CUR_PRES_A;
- }
- } else if (TMR3_COUNT_PRES_CALIB < 4 * TMR_FREQ_5k) {
- V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
- if (TMR3_COUNT_PRES_CALIB >= 3 * TMR_FREQ_5k) {
- CUR_PRES_B_sum += CUR_PRES_B;
- }
- } else {
- CONTROL_MODE = MODE_NO_ACT;
- TMR3_COUNT_PRES_CALIB = 0;
- V_out = 0;
- PRES_SENSOR_A_PULSE_PER_BAR = CUR_PRES_A_sum / ((float) TMR_FREQ_5k - 1.0f) - PRES_A_NULL;
- PRES_SENSOR_A_PULSE_PER_BAR = PRES_SENSOR_A_PULSE_PER_BAR / ((float) PRES_SUPPLY - 1.0f);
- PRES_SENSOR_B_PULSE_PER_BAR = CUR_PRES_B_sum / ((float) TMR_FREQ_5k - 1.0f) - PRES_B_NULL;
- PRES_SENSOR_B_PULSE_PER_BAR = PRES_SENSOR_B_PULSE_PER_BAR / ((float) PRES_SUPPLY - 1.0f);
- CUR_PRES_A_sum = 0;
- CUR_PRES_B_sum = 0;
- CUR_PRES_A_mean = 0;
- CUR_PRES_B_mean = 0;
+// case MODE_PRESSURE_SENSOR_CALIB: {
+// if (TMR3_COUNT_PRES_CALIB < 2 * TMR_FREQ_5k) {
+// V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
+// if (TMR3_COUNT_PRES_CALIB >= TMR_FREQ_5k) {
+// CUR_PRES_A_sum += CUR_PRES_A;
+// }
+// } else if (TMR3_COUNT_PRES_CALIB < 4 * TMR_FREQ_5k) {
+// V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
+// if (TMR3_COUNT_PRES_CALIB >= 3 * TMR_FREQ_5k) {
+// CUR_PRES_B_sum += CUR_PRES_B;
+// }
+// } else {
+// CONTROL_MODE = MODE_NO_ACT;
+// TMR3_COUNT_PRES_CALIB = 0;
+// V_out = 0;
+// PRES_SENSOR_A_PULSE_PER_BAR = CUR_PRES_A_sum / ((float) TMR_FREQ_5k - 1.0f) - PRES_A_NULL;
+// PRES_SENSOR_A_PULSE_PER_BAR = PRES_SENSOR_A_PULSE_PER_BAR / ((float) PRES_SUPPLY - 1.0f);
+// PRES_SENSOR_B_PULSE_PER_BAR = CUR_PRES_B_sum / ((float) TMR_FREQ_5k - 1.0f) - PRES_B_NULL;
+// PRES_SENSOR_B_PULSE_PER_BAR = PRES_SENSOR_B_PULSE_PER_BAR / ((float) PRES_SUPPLY - 1.0f);
+// CUR_PRES_A_sum = 0;
+// CUR_PRES_B_sum = 0;
+// CUR_PRES_A_mean = 0;
+// CUR_PRES_B_mean = 0;
+//
+// ROM_RESET_DATA();
+//
+// //spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f));
+// //spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f));
+// }
+// TMR3_COUNT_PRES_CALIB++;
+// break;
+// }
- ROM_RESET_DATA();
-
- //spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f));
- //spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f));
- }
- TMR3_COUNT_PRES_CALIB++;
- break;
- }
-
- case MODE_ROTARY_FRICTION_TUNING: {
- if (TMR3_COUNT_ROTARY_FRIC_TUNE % (5 * TMR_FREQ_5k) == 0) freq_fric_tune = 4.0f + 3.0f * sin(2 * 3.14159f * 0.5f * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001f * 0.05f);
- V_out = PWM_out * sin(2 * 3.14159f * freq_fric_tune * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001f);
- if (V_out > 0) V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
- else V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
- TMR3_COUNT_ROTARY_FRIC_TUNE++;
- if (TMR3_COUNT_ROTARY_FRIC_TUNE > TUNING_TIME * TMR_FREQ_5k) {
- TMR3_COUNT_ROTARY_FRIC_TUNE = 0;
- V_out = 0.0f;
- CONTROL_MODE = MODE_NO_ACT;
- }
- break;
- }
+// case MODE_ROTARY_FRICTION_TUNING: {
+// if (TMR3_COUNT_ROTARY_FRIC_TUNE % (5 * TMR_FREQ_5k) == 0) freq_fric_tune = 4.0f + 3.0f * sin(2 * 3.14159f * 0.5f * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001f * 0.05f);
+// V_out = PWM_out * sin(2 * 3.14159f * freq_fric_tune * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001f);
+// if (V_out > 0) V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
+// else V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
+// TMR3_COUNT_ROTARY_FRIC_TUNE++;
+// if (TMR3_COUNT_ROTARY_FRIC_TUNE > TUNING_TIME * TMR_FREQ_5k) {
+// TMR3_COUNT_ROTARY_FRIC_TUNE = 0;
+// V_out = 0.0f;
+// CONTROL_MODE = MODE_NO_ACT;
+// }
+// break;
+// }
case MODE_DDV_POS_VS_PWM_ID: {
VALVE_ID_timer = VALVE_ID_timer + 1;
@@ -1356,7 +1313,7 @@
}
CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (6));
-
+
VEL_POINT = 0;
first_check = 1;
DZ_DIRECTION = 1;
@@ -1621,31 +1578,31 @@
break;
}
- if (FLAG_VALVE_OUTPUT_CALIB) {
- // Valve Dead Zone (Mechanical dead-zone canceling)
- // if (CONTROL_MODE != MODE_VALVE_NULLING_AND_DEADZONE_SETTING) {
- // if (V_out > 0.) {
- // VALVE_PWM_VALVE_DZ = (int) V_out + VALVE_DEADZONE_PLUS;
- // } else if (V_out < 0.) {
- // VALVE_PWM_VALVE_DZ = (int) V_out + VALVE_DEADZONE_MINUS;
- // } else VALVE_PWM_VALVE_DZ = (int) V_out + VALVE_CENTER;
- // } else VALVE_PWM_VALVE_DZ = (int) V_out;
-
- if(V_out>0) V_out = V_out + VALVE_DEADZONE_PLUS;
- else if(V_out<0) V_out = V_out + VALVE_DEADZONE_MINUS;
-
- VALVE_PWM_VALVE_DZ = V_out + VALVE_CENTER;
-
- // Output Voltage Linearization and Dead Zone Cancellation (Electrical dead-zone)
- float CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ/5.0f*1000.0f; // convert PWM duty to mV
- float CUR_PWM_DZ = PWM_duty_byLT(CUR_PWM_nonlin);
-
- if (CUR_PWM_DZ > 0) V_out = (int)CUR_PWM_DZ + 143;
- else if (CUR_PWM_DZ < 0) V_out = (int)CUR_PWM_DZ - 138;
- else V_out = CUR_PWM_DZ;
- } else {
- V_out = V_out;
- }
+// if (FLAG_VALVE_OUTPUT_CALIB) {
+// // Valve Dead Zone (Mechanical dead-zone canceling)
+// // if (CONTROL_MODE != MODE_VALVE_NULLING_AND_DEADZONE_SETTING) {
+// // if (V_out > 0.) {
+// // VALVE_PWM_VALVE_DZ = (int) V_out + VALVE_DEADZONE_PLUS;
+// // } else if (V_out < 0.) {
+// // VALVE_PWM_VALVE_DZ = (int) V_out + VALVE_DEADZONE_MINUS;
+// // } else VALVE_PWM_VALVE_DZ = (int) V_out + VALVE_CENTER;
+// // } else VALVE_PWM_VALVE_DZ = (int) V_out;
+//
+// if(V_out>0) V_out = V_out + VALVE_DEADZONE_PLUS;
+// else if(V_out<0) V_out = V_out + VALVE_DEADZONE_MINUS;
+//
+// VALVE_PWM_VALVE_DZ = V_out + VALVE_CENTER;
+//
+// // Output Voltage Linearization and Dead Zone Cancellation (Electrical dead-zone)
+// float CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ/5.0f*1000.0f; // convert PWM duty to mV
+// float CUR_PWM_DZ = PWM_duty_byLT(CUR_PWM_nonlin);
+//
+// if (CUR_PWM_DZ > 0) V_out = (int)CUR_PWM_DZ + 143;
+// else if (CUR_PWM_DZ < 0) V_out = (int)CUR_PWM_DZ - 138;
+// else V_out = CUR_PWM_DZ;
+// } else {
+// V_out = V_out;
+// }
/*******************************************************
*** PWM
@@ -1695,7 +1652,7 @@
if (flag_data_request[0] == HIGH) {
//position+velocity
//CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen);
- CAN_TX_POSITION((int16_t) (pos.sen/4.0f), (int16_t) (V_out/100.0f), (int16_t) (torq.sen*10.0f));
+ CAN_TX_POSITION((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (torq.sen*10.0f));
}
if (flag_data_request[1] == HIGH) {
@@ -1703,9 +1660,9 @@
//CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM * 100.));
//CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM));
double t_value = 0;
- if(value>=DDV_CENTER){
+ if(value>=DDV_CENTER) {
t_value = 10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER);
- }else{
+ } else {
t_value = -10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
}
CAN_TX_TORQUE((int16_t) (t_value));
@@ -1713,7 +1670,7 @@
if (flag_data_request[2] == HIGH) {
//pressure A and B
- CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (PRES_A_VREF * 1000.0f)); // CUR_PRES_X : 0(0bar)~4096(210bar)
+ CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (MODE_POS_FT_TRANS * 100.0f)); // CUR_PRES_X : 0(0bar)~4096(210bar)
//CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // 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.);
@@ -1741,7 +1698,7 @@
// CAN_TX_VALVE_POSITION((int16_t) Ref_Joint_Pos);
// CAN_TX_VALVE_POSITION((int16_t) flag_flowrate);
}
-
+
}
TIM2->SR = 0x0; // reset the status register
@@ -1788,3 +1745,4 @@
+