Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2_copy
2011
Diff: main.cpp
- Revision:
- 213:9c0becc196ba
- Parent:
- 212:985dba42f261
- Child:
- 214:82a3ca333004
diff -r 985dba42f261 -r 9c0becc196ba main.cpp --- a/main.cpp Mon Dec 28 14:27:11 2020 +0000 +++ b/main.cpp Thu Feb 04 05:43:47 2021 +0000 @@ -88,6 +88,7 @@ extern int CID_TX_PRES; extern int CID_TX_VOUT; extern int CID_TX_VALVE_POSITION; +extern int CID_TX_SOMETHING; /******************************************************************************* @@ -1687,48 +1688,52 @@ TIM4->CCR2 = (PWM_ARR)*(1.0f-dtc_v); TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w); - + if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) { - // Position, Velocity, and Torque (ID:1200) - 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)); - } else if (SENSING_MODE == 1) { - 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/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE))); - } else if (SENSING_MODE == 1) { - CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f)); - } - } - } + // Position, Velocity, and Torque (ID:1200) + 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)); + } else if (SENSING_MODE == 1) { + 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/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE))); + } else if (SENSING_MODE == 1) { + CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f)); + } + } + + } + // ID:1300 if (flag_data_request[1] == HIGH) { CAN_TX_TORQUE((int16_t) 7); //1300 } - // ID:1400 + // Reference Current, Current Current (ID:1700) if (flag_data_request[2] == HIGH) { - double t_value = 0.0f; - 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)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); - } - double t_value_ref = 0.0f; - if(valve_pos.ref>=(float) VALVE_CENTER) { - t_value_ref = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); - } else { - t_value_ref = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); - } - - - CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400 + + CAN_TX_SOMETHING((int16_t) (I_REF_fil / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse), (int16_t) (0), (int16_t) (0)); + //double t_value = 0.0f; + // 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)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); + // } + // double t_value_ref = 0.0f; + // if(valve_pos.ref>=(float) VALVE_CENTER) { + // t_value_ref = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); + // } else { + // t_value_ref = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); + // } + // + // + // CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400 } //If it doesn't rest, below can can not work.