Sungwoo Kim
Original Version of STM Board
Diff: main.cpp
- Revision:
- 52:8ea76864368a
- Parent:
- 51:b46bed7fec80
- Child:
- 53:4d66fb1c5dd9
- Child:
- 54:647072f5307a
diff -r b46bed7fec80 -r 8ea76864368a main.cpp
--- a/main.cpp Wed Feb 19 00:44:07 2020 +0000
+++ b/main.cpp Wed Feb 19 05:48:57 2020 +0000
@@ -180,9 +180,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);
@@ -191,7 +191,7 @@
//Timer priority
NVIC_SetPriority(TIM3_IRQn, 2);
- NVIC_SetPriority(TIM2_IRQn, 3);
+// NVIC_SetPriority(TIM2_IRQn, 3);
NVIC_SetPriority(TIM4_IRQn, 4);
//can.reset();
@@ -1040,25 +1040,6 @@
valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
VALVE_POS_CONTROL(valve_pos.ref);
-
-// if((OPERATING_MODE && 0x01) == 0) { // Rotary Mode
-// float wn_Pos = 2.0f*PI*5.0f; // f_cut : 10Hz Position Control
-// float temp_vel = ( 0.01f * (float)P_GAIN_JOINT_POSITION * wn_Pos * pos.err)*PI/180.0f; // rad/s
-// // L when P-gain = 100, f_cut = 10Hz
-// if (temp_vel > 0.0f ) I_REF = temp_vel*((float)PISTON_AREA_A*0.00006f/(K_v*sqrt(2.0f*alpha3/(alpha3+1.0f))));
-// else I_REF = temp_vel*((float)PISTON_AREA_B*0.00006f/(K_v*sqrt(2.0f/(alpha3+1.0f))));
-// // ------------------------------------------------------------------------
-// // L thetadot(rad/s) >> I_ref(mA)
-// } else if ((OPERATING_MODE && 0x01) == 1) { // Linear Mode
-// float wn_Pos = 2.0f*PI*5.0f; // f_cut : 10Hz Position Control
-// float temp_vel = ( 0.01f * (float)P_GAIN_JOINT_POSITION * wn_Pos * pos.err); // mm/s
-// // L when P-gain = 100, f_cut = 10Hz
-// if (temp_vel > 0.0f ) I_REF = temp_vel*((float)PISTON_AREA_A*0.00006f/(K_v*sqrt(2.0f*alpha3/(alpha3+1.0f))));
-// else I_REF = temp_vel*((float)PISTON_AREA_B*0.00006f/(K_v*sqrt(2.0f/(alpha3+1.0f))));
-// // ------------------------------------------------------------------------
-// // L xdot(mm/s) >> I_ref(mA)
-// }
-
cnt_findhome++;
if (cnt_findhome >= T_move) {
//REFERENCE_MODE = MODE_REF_DIRECT;
@@ -1650,31 +1631,29 @@
TIM4->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
- }
- 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)
-{
- if (TIM2->SR & TIM_SR_UIF ) {
+
+ if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k / CAN_FREQ) == 0) {
- //CAN ----------------------------------------------------------------------
+ // Position, Velocity, and Torque (ID:1200)
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) (vel.sen/100.0f), (int16_t) (torq.sen*10.0f));
+ if ((OPERATING_MODE & 0x01) == 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
+ 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) {
+ 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));
+ }
+ }
}
-
if (flag_data_request[1] == HIGH) {
- //torque
- //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM * 100.));
- //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM));
+ //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);
@@ -1687,41 +1666,98 @@
if (flag_data_request[2] == HIGH) {
//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] == 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] == HIGH) {
//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) (DDV_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);
- }
+ // Others : Reference position, Reference FT, PWM, Current (ID:1300)
+// if (flag_data_request[1] == HIGH) {
+// CAN_TX_SOMETHING((int) (FORCE_VREF), (int16_t) (1), (int16_t) (2), (int16_t) (3));
+// }
+
+ TMR2_COUNT_CAN_TX = 0;
+ }
+
}
- TIM2->SR = 0x0; // reset the status register
+ 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] == HIGH) {
+// //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) {
+// //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);
+// } else {
+// t_value = -10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
+// }
+// CAN_TX_TORQUE((int16_t) (t_value));
+// }
+//
+// if (flag_data_request[2] == HIGH) {
+// //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] == 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] == HIGH) {
+// //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) (DDV_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;