Sungwoo Kim
distribution-201229
Diff: main.cpp
- Revision:
- 55:b25725257569
- Parent:
- 54:647072f5307a
- Child:
- 56:6f50d9d3bfee
--- a/main.cpp Tue Feb 25 12:56:39 2020 +0000
+++ b/main.cpp Wed Feb 26 12:51:52 2020 +0000
@@ -136,7 +136,7 @@
/*********************************
*** Initialization
*********************************/
- //LED = 1;
+ LED = 1;
//pc.baud(9600);
// i2c init
@@ -172,9 +172,9 @@
make_delay();
// TMR2 init
-// Init_TMR2();
-// TIM2->CR1 ^= TIM_CR1_UDIS;
-// make_delay();
+ Init_TMR2();
+ TIM2->CR1 ^= TIM_CR1_UDIS;
+ make_delay();
// CAN
can.attach(&CAN_RX_HANDLER);
@@ -183,7 +183,7 @@
//Timer priority
NVIC_SetPriority(TIM3_IRQn, 2);
-// NVIC_SetPriority(TIM2_IRQn, 3);
+ NVIC_SetPriority(TIM2_IRQn, 3);
NVIC_SetPriority(TIM4_IRQn, 3);
//can.reset();
@@ -206,7 +206,7 @@
else
ID_index_array[i] = (i+1) * 0.5f;
}
-
+
/************************************
*** Program is operating!
*************************************/
@@ -234,15 +234,15 @@
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]) {
- Ref_Valve_Pos_FF = DDV_CENTER;
+ Ref_Valve_Pos_FF = (float) VALVE_CENTER;
} else {
- Ref_Valve_Pos_FF = ((float) 10/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER;
+ Ref_Valve_Pos_FF = ((float) 10/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + (float) VALVE_CENTER;
}
} else {
if(JOINT_VEL[i+1] == JOINT_VEL[i-1]) {
- Ref_Valve_Pos_FF = DDV_CENTER;
+ Ref_Valve_Pos_FF = (float) VALVE_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]);
+ 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])) + (float) VALVE_CENTER + (float) (10*ID_index_array[i-1]);
}
}
break;
@@ -254,7 +254,7 @@
Ref_Valve_Pos_FF = (float) VALVE_MIN_POS;
}
- Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER);
+ Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - (float) VALVE_CENTER);
return Ref_Valve_Pos_FF;
}
@@ -367,10 +367,10 @@
//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;
+ 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
@@ -393,11 +393,13 @@
float DT_TMR3 = (float)DT_5k;
int cnt_trans = 0;
double VALVE_POS_RAW_FORCE_FB_LOGGING = 0.0f;
-int canfreq = CAN_FREQUENCY;
+int can_rest =0;
extern "C" void TIM3_IRQHandler(void)
{
if (TIM3->SR & TIM_SR_UIF ) {
+
+
if ((OPERATING_MODE&0b110) == 0) {
K_v = 0.4f; // Moog (LPM >> mA) , 100bar
@@ -591,7 +593,7 @@
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 * pos.err/(float) ENC_PULSE_PER_POSITION * 0.01f;
- valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
+ valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER;
VALVE_POS_CONTROL(valve_pos.ref);
//float wn_Pos = 2.0f*PI*5.0f; // f_cut : 10Hz Position Control
@@ -621,7 +623,7 @@
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/(float) ENC_PULSE_PER_POSITION;
- valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
+ valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER;
VALVE_POS_CONTROL(valve_pos.ref);
cnt_findhome++;
@@ -982,7 +984,7 @@
DZ_index= DZ_index *2;
if(DZ_index >= 128) {
SECOND_DZ = valve_pos.ref;
- DDV_CENTER = 0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ);
+ VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
first_check = 0;
VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
@@ -1067,7 +1069,7 @@
DZ_index= DZ_index *2;
if(DZ_index >= 128) {
SECOND_DZ = valve_pos.ref;
- DDV_CENTER = 0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ);
+ VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
first_check = 0;
VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
@@ -1099,7 +1101,7 @@
pos_minus_end = pos.sen;
first_check = 1;
VALVE_FR_timer = 0;
- valve_pos.ref = DDV_CENTER;
+ valve_pos.ref = (float) VALVE_CENTER;
ID_index = 0;
max_check = 0;
min_check = 0;
@@ -1109,12 +1111,12 @@
V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
} else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
data_num = 0;
- valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + DDV_CENTER;
+ valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + (float) VALVE_CENTER;
VALVE_POS_CONTROL(valve_pos.ref);
START_POS = pos.sen;
} else if(VALVE_FR_timer < (int) (5.0f * (float) TMR_FREQ_5k)) {
- valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + DDV_CENTER;
+ valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + (float) VALVE_CENTER;
VALVE_POS_CONTROL(valve_pos.ref);
data_num = data_num + 1;
if(abs(0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen) > 20000.0f) {
@@ -1276,7 +1278,7 @@
double KI_I = 0.1f * R_model*w0;
// KNR Valve Current Control Gain
- if ((OPERATING_MODE & 0x02) == 1) { // KNR Valve
+ if ((OPERATING_MODE & 0b110) == 1) { // KNR Valve
R_model = 163.0f; // ohm
L_model = 1.0f;
w0 = 2.0f * 3.14f * 80.0f;
@@ -1319,7 +1321,7 @@
if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS * mV_PER_pulse; // unit: mV
else if (VALVE_PWM_RAW < 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS * mV_PER_pulse; // unit: mV
- VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + VALVE_CENTER * mV_PER_pulse; // unit: mV
+ VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + (double)VALVE_CENTER * mV_PER_pulse; // unit: mV
} else {
VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW;
@@ -1364,19 +1366,17 @@
TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
+ if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
-
- if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/canfreq) == 0) {
-// if (TMR2_COUNT_CAN_TX % 10 == 0) {
// Position, Velocity, and Torque (ID:1200)
- if (flag_data_request[0] == HIGH) {
- if ((OPERATING_MODE & 0x01) == 0) { // Rotary Actuator
+ if (flag_data_request[0] == LOW) {
+ 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));
} 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));
}
- } else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator
+ } 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));
} else if (SENSING_MODE == 1) {
@@ -1384,30 +1384,38 @@
}
}
}
- if (flag_data_request[1] == HIGH) {
+ if (flag_data_request[1] == LOW) {
//valve position
double t_value = 0;
- if(value>=DDV_CENTER) {
- t_value = 10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER);
+ if(value>=(float) 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)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
+ t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
}
- CAN_TX_TORQUE((int16_t) (t_value));
+ CAN_TX_TORQUE((int16_t) (t_value)); //1300
}
-
- if (flag_data_request[2] == HIGH) {
+
+
+ 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)
+ CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar) //1400
}
-
- if (flag_data_request[3] == HIGH) {
+
+ //If it doesn't rest, below can can not work.
+ for (can_rest = 0; can_rest < 10000; can_rest++) {
+ ;
+ }
+
+ if (flag_data_request[3] == LOW) {
//PWM
- CAN_TX_PWM((int16_t) VALVE_DEADZONE_PLUS);
+ CAN_TX_PWM((int16_t) cur.sen); //1500
}
-
- if (flag_data_request[4] == HIGH) {
+ //for (i = 0; i < 10000; i++) {
+// ;
+// }
+ if (flag_data_request[4] == LOW) {
//valve position
- 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) (CAN_FREQ), (int16_t) (D_DAMPER), (int16_t) OPERATING_MODE); //1600
}
// Others : Reference position, Reference FT, PWM, Current (ID:1300)
@@ -1422,8 +1430,6 @@
}
TMR2_COUNT_CAN_TX++;
-
-
}
TIM3->SR = 0x0; // reset the status register
@@ -1434,58 +1440,63 @@
//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] == HIGH) {
+// //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] == HIGH) {
+// //if (flag_data_request[1] == LOW) {
// //valve position
// double t_value = 0;
-// if(value>=DDV_CENTER) {
-// t_value = 10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER);
+// 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)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
+// 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] == HIGH) {
+// //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)
-// //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.);
-// // CAN_TX_PRES((int16_t) DZ_dir, (int16_t) ((VALVE_DEADZONE_PLUS + 1.))*1000.);
+//
+// //}
//
+// //if (flag_data_request[3] == LOW) {
+// //PWM
+// int i = 0;
+// for (i = 0; i < 10000; i++) {
+// ;
// }
-//
-// if (flag_data_request[3] == HIGH) {
-// //PWM
// 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
//
-// if (flag_data_request[4] == HIGH) {
-// //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) (DDV_CENTER * 10.0f), (int16_t) valve_pos.ref, (int16_t) V_out);
+// //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);
-// }
+// //}
//
//
// }