![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
LIGHT2
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;