Sungwoo Kim
distribution-201229
Diff: main.cpp
- Revision:
- 58:2eade98630e2
- Parent:
- 57:f4819de54e7a
- Child:
- 59:f308b1656d9c
--- a/main.cpp Fri Mar 06 12:14:38 2020 +0000
+++ b/main.cpp Mon Mar 09 12:43:04 2020 +0000
@@ -194,10 +194,13 @@
make_delay();
//DAC init
-// dac_1 = PRES_A_VREF / 3.3f;
-// dac_2 = PRES_B_VREF / 3.3f;
- dac_1 = TORQUE_VREF / 3.3f;
- dac_2 = 0.0f;
+ if (SENSING_MODE == 0) {
+ dac_1 = TORQUE_VREF / 3.3f;
+ dac_2 = 0.0f;
+ } else if (SENSING_MODE == 1) {
+ dac_1 = PRES_A_VREF / 3.3f;
+ dac_2 = PRES_B_VREF / 3.3f;
+ }
make_delay();
for (int i=0; i<50; i++) {
@@ -206,7 +209,7 @@
else
ID_index_array[i] = (i+1) * 0.5f;
}
-
+
/************************************
*** Program is operating!
*************************************/
@@ -356,21 +359,46 @@
if (CNT_TMR4 % (int) ((int) FREQ_TMR4/TMR4_FREQ_10k) == 0) {
ENC_UPDATE();
}
+
+ ADC1->CR2 |= 0x40000000;
+ if (SENSING_MODE == 0) {
+ // Torque Sensing (0~210)bar =============================================
+ float pres_A_new = (((float) ADC1->DR) - 2047.5f);
+ double alpha_update_ft = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 100.0f)); // f_cutoff : 200Hz
+ pres_A.sen = (1.0f - alpha_update_ft) * pres_A.sen + alpha_update_ft * pres_A_new;
+ torq.sen = pres_A.sen / TORQUE_SENSOR_PULSE_PER_TORQUE;
- //Pressure sensor A
- ADC1->CR2 |= 0x40000000; // adc _ 12bit
- //while((ADC1->SR & 0b10));
- float alpha_update_pres_A = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*100.0f));
- 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 //SW just changed the sign to correct the direction of loadcell on LIGHT. Correct later.
+ } else if (SENSING_MODE == 1) {
+ // Pressure Sensing (0~210)bar =============================================
+ float pres_A_new = (((float)ADC1->DR) - PRES_A_NULL);
+ float pres_B_new = (((float)ADC2->DR) - PRES_B_NULL);
+ double alpha_update_pres = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 200.0f)); // f_cutoff : 500Hz
+ pres_A.sen = (1.0f - alpha_update_pres) * pres_A.sen + alpha_update_pres * pres_A_new;
+ pres_B.sen = (1.0f - alpha_update_pres) * pres_B.sen + alpha_update_pres * pres_B_new;
+ CUR_PRES_A_BAR = pres_A.sen / PRES_SENSOR_A_PULSE_PER_BAR;
+ CUR_PRES_B_BAR = pres_B.sen / PRES_SENSOR_B_PULSE_PER_BAR;
-
- //Pressure sensor B
- float alpha_update_pres_B = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*100.0f));
- float pres_B_new = ((float)ADC2->DR);
- pres_B.sen = pres_B.sen*(1.0f-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B);
- //torq.sen = pres_A.sen * (float) PISTON_AREA_A - pres_B.sen * (float) PISTON_AREA_B;
+ if ((OPERATING_MODE & 0x01) == 0) { // Rotary Actuator
+ torq.sen = (PISTON_AREA_A * CUR_PRES_A_BAR - PISTON_AREA_B * CUR_PRES_B_BAR) * 0.0001f; // mm^3*bar >> Nm
+ } else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator
+ torq.sen = (PISTON_AREA_A * CUR_PRES_A_BAR - PISTON_AREA_B * CUR_PRES_B_BAR) * 0.1f; // mm^2*bar >> N
+ }
+ }
+
+// //Pressure sensor A
+// ADC1->CR2 |= 0x40000000; // adc _ 12bit
+// //while((ADC1->SR & 0b10));
+// float alpha_update_pres_A = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*100.0f));
+// 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 //SW just changed the sign to correct the direction of loadcell on LIGHT. Correct later.
+//
+//
+// //Pressure sensor B
+// float alpha_update_pres_B = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*100.0f));
+// float pres_B_new = ((float)ADC2->DR);
+// pres_B.sen = pres_B.sen*(1.0f-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B);
+// //torq.sen = pres_A.sen * (float) PISTON_AREA_A - pres_B.sen * (float) PISTON_AREA_B;
//Current
@@ -381,7 +409,7 @@
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;
-
+
CNT_TMR4++;
}
TIM4->SR = 0x0; // reset the status register
@@ -398,8 +426,6 @@
extern "C" void TIM3_IRQHandler(void)
{
if (TIM3->SR & TIM_SR_UIF ) {
-
-
if (((OPERATING_MODE&0b110)>>1) == 0) {
K_v = 0.4f; // Moog (LPM >> mA) , 100bar
@@ -426,17 +452,17 @@
if((float) cnt_trans * DT_TMR3 > 3.0f )
MODE_POS_FT_TRANS = 0;
} else if(MODE_POS_FT_TRANS == 2) {
- alpha_trans = 1.0;
+ alpha_trans = 1.0f;
cnt_trans = 0;
} else {
- alpha_trans = 0.0;
+ alpha_trans = 0.0f;
cnt_trans = 0;
}
int UTILITY_MODE = 0;
int CONTROL_MODE = 0;
-
+
if (CONTROL_UTILITY_MODE >= 20 || CONTROL_UTILITY_MODE == 0) {
UTILITY_MODE = CONTROL_UTILITY_MODE;
CONTROL_MODE = MODE_NO_ACT;
@@ -472,7 +498,7 @@
dac_1 = TORQUE_VREF / 3.3f;
}
} else {
- CONTROL_MODE = MODE_NO_ACT;
+ CONTROL_UTILITY_MODE = MODE_NO_ACT;
TMR3_COUNT_TORQUE_NULL = 0;
CUR_TORQUE_sum = 0;
CUR_TORQUE_mean = 0;
@@ -713,7 +739,6 @@
// }
//
// }
-
case MODE_PRESSURE_SENSOR_NULLING: {
// DAC Voltage reference set
if (TMR3_COUNT_PRES_NULL < TMR_FREQ_5k * 2) {
@@ -727,8 +752,8 @@
CUR_PRES_B_sum = 0;
float VREF_NullingGain = 0.0003f;
- PRES_A_VREF = PRES_A_VREF - VREF_NullingGain * (PRES_A_NULL - CUR_PRES_A_mean);
- PRES_B_VREF = PRES_B_VREF - VREF_NullingGain * (PRES_B_NULL - CUR_PRES_B_mean);
+ PRES_A_VREF = PRES_A_VREF + VREF_NullingGain * CUR_PRES_A_mean;
+ PRES_B_VREF = PRES_B_VREF + VREF_NullingGain * CUR_PRES_B_mean;
if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f;
if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f;
@@ -1156,7 +1181,7 @@
}
break;
}
-
+
case MODE_SYSTEM_ID: {
freq_sysid_Iref = (double) cnt_sysid * DT_TMR3 * 3.;
valve_pos.ref = 2500.0f * sin(2.0f * 3.14159f * freq_sysid_Iref * (double) cnt_sysid * DT_TMR3);
@@ -1168,8 +1193,8 @@
}
break;
}
-
-
+
+
default:
break;
@@ -1191,7 +1216,7 @@
} else { //SW Valve
VALVE_POS_CONTROL(valve_pos.ref);
}
-
+
break;
}
@@ -1203,11 +1228,11 @@
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)/(float)(TORQUE_SENSOR_PULSE_PER_TORQUE); //[N]
+ torq.err = (torq_ref)/(float)(TORQUE_SENSOR_PULSE_PER_TORQUE) - torq.sen; //[N]
torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N]
if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) {
-
+
double I_REF_POS = 0.0f;
double I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback
double I_REF_VC = 0.0f; // I_REF for velocity compensation
@@ -1215,7 +1240,7 @@
double temp_vel_pos = 0.0f;
double temp_vel_torq = 0.0f;
double wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control
-
+
if ((OPERATING_MODE && 0x01) == 0) { // Rotary Mode
temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION) * PI / 180.0f; // rad/s
// L when P-gain = 100, f_cut = 10Hz L feedforward velocity
@@ -1275,10 +1300,10 @@
VALVE_POS_CONTROL(valve_pos.ref);
}
-
+
break;
}
-
+
case MODE_VALVE_OPEN_LOOP: {
V_out = (float) Vout.ref;
break;
@@ -1290,7 +1315,7 @@
if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve
-
+
////////////////////////////////////////////////////////////////////////////
//////////////////////////// CURRENT CONTROL //////////////////////////////
////////////////////////////////////////////////////////////////////////////
@@ -1368,8 +1393,8 @@
else if (CUR_PWM_lin < 0) V_out = (int) CUR_PWM_lin - 140;
else V_out = CUR_PWM_lin;
}
-
-
+
+
/*******************************************************
*** PWM
@@ -1404,15 +1429,15 @@
if (flag_data_request[0] == HIGH) {
if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
if (SENSING_MODE == 0) {
- CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f));
+ CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (pres_A.sen*10.0f));
} else if (SENSING_MODE == 1) {
- CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (pres_A.sen*10.0f), (int16_t) (pres_B.sen*10.0f));
+ CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
}
} else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
if (SENSING_MODE == 0) {
- CAN_TX_POSITION_FT((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (torq.sen*10.0f));
+ CAN_TX_POSITION_FT((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (pres_A.sen*10.0f));
} else if (SENSING_MODE == 1) {
- CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (pres_A.sen*10.0f), (int16_t) (pres_B.sen*10.0f));
+ CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
}
}
}
@@ -1424,27 +1449,27 @@
// } else {
// t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
// }
- if(OPERATING_MODE==5){
+ if(OPERATING_MODE==5) {
t_value = (double) value;
- }else if(CURRENT_CONTROL_MODE==1){
+ } else if(CURRENT_CONTROL_MODE==1) {
t_value = cur.sen;
- }else{
+ } else {
t_value = V_out;
}
CAN_TX_TORQUE((int16_t) (t_value)); //1300
}
-
-
+
+
if (flag_data_request[2] == HIGH) {
//pressure A and B
CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar) //1400
}
-
+
//If it doesn't rest, below can can not work.
for (can_rest = 0; can_rest < 10000; can_rest++) {
;
}
-
+
if (flag_data_request[3] == HIGH) {
//PWM
CAN_TX_PWM((int16_t) cur.sen); //1500
@@ -1462,7 +1487,7 @@
// CAN_TX_SOMETHING((int) (FORCE_VREF), (int16_t) (1), (int16_t) (2), (int16_t) (3));
// }
//if (flag_delay_test == 1){
- //CAN_TX_PRES((int16_t) (0),(int16_t) torq_ref);
+ //CAN_TX_PRES((int16_t) (0),(int16_t) torq_ref);
//}
TMR2_COUNT_CAN_TX = 0;
@@ -1472,115 +1497,4 @@
}
TIM3->SR = 0x0; // reset the status register
-}
-
-
-
-//unsigned long CNT_TMR5 = 0;
-//float FREQ_TMR5 = (float)FREQ_500;
-//float DT_TMR5 = (float)DT_500;
-
-//extern "C" void TIM2_IRQHandler(void)
-//{
-// LED = 1;
-// if (TIM2->SR & TIM_SR_UIF ) {
-//
-// //CAN ----------------------------------------------------------------------
-// //if (flag_data_request[0] == LOW) {
-// //position+velocity
-// CAN_TX_POSITION_FT((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (torq.sen*10.0f));
-// //}
-//
-// //if (flag_data_request[1] == LOW) {
-// //valve position
-// double t_value = 0;
-// if(value>=(double)VALVE_CENTER) {
-// t_value = 10000.0f*((double)value-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER);
-// } else {
-// t_value = -10000.0f*((double)value-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER);
-// }
-// CAN_TX_TORQUE((int16_t) (t_value));
-// //}
-//
-// //if (flag_data_request[2] == LOW) {
-// //pressure A and B
-// CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (MODE_POS_FT_TRANS * 100.0f)); // CUR_PRES_X : 0(0bar)~4096(210bar)
-//
-// //}
-//
-// //if (flag_data_request[3] == LOW) {
-// //PWM
-// int i = 0;
-// for (i = 0; i < 10000; i++) {
-// ;
-// }
-// CAN_TX_PWM((int16_t) VALVE_DEADZONE_PLUS);
-// // 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] == LOW) {
-// //valve position
-//
-// for (i = 0; i < 10000; i++) {
-// ;
-// }
-// CAN_TX_VALVE_POSITION((int16_t) (K_SPRING), (int16_t) (D_DAMPER), (int16_t) VALVE_POS_RAW_FORCE_FB_LOGGING);
-// //CAN_TX_VALVE_POSITION((int16_t) ((float) VALVE_CENTER * 10.0f), (int16_t) valve_pos.ref, (int16_t) V_out);
-//
-// //CAN_TX_VALVE_POSITION((int16_t) (VALVE_POS_NUM));
-// // CAN_TX_VALVE_POSITIOfxN((int16_t) (VALVE_FF_VOLTAGE / 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);
-// //}
-//
-//
-// }
-// TIM2->SR = 0x0; // reset the status register
-//}
-
-
-//
-//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;
-// cur.err_old = cur.err;
-//
-// float R_model = 150.0f; // ohm
-// float L_model = 0.3f;
-// float w0 = 2.0f*3.14f*90.0f;
-// float KP_I = L_model*w0;
-// float KI_I = R_model*w0;
-// float KD_I = 0.0f;
-//
-// float FF_gain = 0.0f;
-// 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
-//
-// float Ka = 5.0f/KP_I;
-// if(V_out > V_MAX) {
-// V_rem = V_out-V_MAX;
-// V_rem = Ka*V_rem;
-// V_out = V_MAX;
-// 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_out = -V_MAX;
-// cur.err_int = cur.err_int - V_rem*DT_5k;
-// }
-//}
-
-
-
-
-
-
-
-
+}
\ No newline at end of file