Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2
rainbow
Revision 242:b235d67d25ba, committed 2022-04-07
- Comitter:
- Lightvalve
- Date:
- Thu Apr 07 06:18:38 2022 +0000
- Parent:
- 241:27eca07c9591
- Child:
- 243:30896263bd8b
- Commit message:
- 220407
Changed in this revision
CAN/function_CAN.cpp | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/CAN/function_CAN.cpp Tue Apr 05 09:07:14 2022 +0000 +++ b/CAN/function_CAN.cpp Thu Apr 07 06:18:38 2022 +0000 @@ -674,8 +674,12 @@ void CAN_RX_HANDLER() { - + + if (LED > 0) LED = 0; + else LED = 1; + can.read(msg); + unsigned int address = msg.id; if(address==CID_RX_CMD) { unsigned int CMD = msg.data[0]; @@ -749,7 +753,6 @@ Vout.ref = (double) temp_ref_pwm; } - } /******************************************************************************
--- a/main.cpp Tue Apr 05 09:07:14 2022 +0000 +++ b/main.cpp Thu Apr 07 06:18:38 2022 +0000 @@ -59,10 +59,10 @@ // CAN /////////////////////////////////////////// CAN can(PB_8, PB_9, 1000000); CANMessage msg; -void onMsgReceived() -{ - CAN_RX_HANDLER(); -} +//void onMsgReceived() +//{ +// CAN_RX_HANDLER(); +//} // Variables /////////////////////////////////////////// State pos; @@ -263,7 +263,9 @@ make_delay(); //can.reset(); - can.filter(msg.id, 0xFFFFF000, CANStandard); +// can.filter(msg.id, 0xFFFFF000, CANStandard); +// can.filter(0x050, 0xFFFFFFFF, CANStandard); + can.filter(0b100000000, 0b100000010, CANStandard); //CAN ID 100~400번대 통과하게 // TMR3 init Init_TMR3(); @@ -406,11 +408,13 @@ REF_VALVE_POS_10000 = -10000.0f; } if (REF_VALVE_POS_10000 >= 0) { - valve_pos_ref_dz = REF_VALVE_POS_10000/10000.0f * (VALVE_MAX_POS-VALVE_DEADZONE_PLUS) + VALVE_DEADZONE_PLUS; +// valve_pos_ref_dz = REF_VALVE_POS_10000/10000.0f * (VALVE_MAX_POS-VALVE_DEADZONE_PLUS) + VALVE_DEADZONE_PLUS; + valve_pos_ref_dz = REF_VALVE_POS_10000; } else { - valve_pos_ref_dz = -REF_VALVE_POS_10000/10000.0f * (VALVE_MIN_POS-VALVE_DEADZONE_MINUS) + VALVE_DEADZONE_MINUS; +// valve_pos_ref_dz = -REF_VALVE_POS_10000/10000.0f * (VALVE_MIN_POS-VALVE_DEADZONE_MINUS) + VALVE_DEADZONE_MINUS; + valve_pos_ref_dz = REF_VALVE_POS_10000; } - valve_pos_err = (float) (valve_pos_ref_dz - value); + valve_pos_err = (float) (valve_pos_ref_dz - valve_pos.sen); valve_pos_err_diff = valve_pos_err - valve_pos_err_old; valve_pos_err_old = valve_pos_err; valve_pos_err_sum += valve_pos_err; @@ -515,6 +519,7 @@ float alpha_LVDT = 1.0f/(1.0f+TMR_FREQ_1k/(2.0f*PI*300.0f)); LVDT_LPF = (1.0f-alpha_LVDT) * LVDT_LPF + alpha_LVDT * LVDT_new; valve_pos.sen = LVDT_LPF; + if(DIR_VALVE_ENC < 0) valve_pos.sen = 0.0f - valve_pos.sen; } TIM3->SR = 0x0; // reset the status register @@ -823,7 +828,8 @@ data_num = 0; } else if(VALVE_ID_timer < TMR_FREQ_5k*3) { data_num = data_num + 1; - VALVE_POS_TMP = VALVE_POS_TMP + value; +// VALVE_POS_TMP = VALVE_POS_TMP + value; + VALVE_POS_TMP = VALVE_POS_TMP + valve_pos.sen; } else if(VALVE_ID_timer == TMR_FREQ_5k*3) { Vout.ref = 0.0f; } else { @@ -1529,8 +1535,8 @@ ////////////////////// Data transmission through CAN ////////////////////// //////////////////////////////////////////////////////////////////////////// - if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) { - + if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) { + // Position, Velocity, and Torque (ID:1200) // if (flag_data_request[0] == LOW) { @@ -1541,7 +1547,7 @@ // } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator // CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); - CAN_TX_POSITION_FT((int16_t) (7.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (valve_pos.sen)); + CAN_TX_POSITION_FT((int16_t) (7.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (valve_pos.sen)); // } // }