Sungwoo Kim
/
HydraulicControlBoard_Learning
for learning
Diff: main.cpp
- Revision:
- 20:806196fda269
- Parent:
- 19:23b7c1ad8683
- Child:
- 21:e5f1a43ea6f9
diff -r 23b7c1ad8683 -r 806196fda269 main.cpp --- a/main.cpp Mon Sep 09 06:29:56 2019 +0000 +++ b/main.cpp Mon Sep 09 06:57:31 2019 +0000 @@ -180,6 +180,11 @@ TIM3->CR1 ^= TIM_CR1_UDIS; make_delay(); + // TMR5 init + Init_TMR5(); + TIM5->CR1 ^= TIM_CR1_UDIS; + make_delay(); + // CAN can.attach(&CAN_RX_HANDLER); CAN_ID_INIT(); @@ -1588,7 +1593,7 @@ //VALVE_PWM(CUR_PWM, VALVE_VOLTAGE_LIMIT, SUPPLY_VOLTAGE); //PWM_out = CUR_PWM; - +/* //CAN ---------------------------------------------------------------------- //if (TMR3_COUNT_CAN_TX % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) { @@ -1651,7 +1656,7 @@ } TMR3_COUNT_CAN_TX++; - +*/ /******************************************************* @@ -1728,6 +1733,74 @@ } + + +//unsigned long CNT_TMR5 = 0; +//double FREQ_TMR5 = (double)FREQ_500; +//double DT_TMR5 = (double)DT_500; +extern "C" void TIM5_IRQHandler(void) +{ + if (TIM5->SR & TIM_SR_UIF ) { + + //CAN ---------------------------------------------------------------------- + if (flag_data_request[0] == HIGH) { + //position+velocity + CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen); + //CAN_TX_POSITION((int32_t) valve_pos.ref, (int32_t) 0); + //CAN_TX_POSITION((int32_t) VALVE_PWM_RAW_FF, (int32_t) VALVE_POS_VS_PWM[10]); + //pc.printf("can good"); + // CAN_TX_POSITION((long) CUR_PRES_A_BAR, (long) CUR_PRES_B_BAR); + } + + if (flag_data_request[1] == HIGH) { + //torque + //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM * 100.)); + //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM)); + CAN_TX_TORQUE((int16_t) (cur.sen)); + //CAN_TX_TORQUE((int16_t) (Ref_Valve_Pos_FF_CAN)); + // CAN_TX_TORQUE((int16_t) DZ_temp_cnt); + } + + if (flag_data_request[2] == HIGH) { + //pressure A and B + //CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (PRES_A_VREF)); // 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) CUR_PWM); + //CAN_TX_PWM((int16_t) (Ref_Valve_Pos_FF_CAN)); + // 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) (value)); + //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF)); + //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0_1, (int16_t) ((0xFEF1>>16) & 0xFFFF)); + + + //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); + } + + } + TIM5->SR = 0x0; // reset the status register +} + + + void CurrentControl() { cur.err = cur.ref - cur.sen;