Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2_
rainbow
Diff: main.cpp
- Revision:
- 242:b235d67d25ba
- Parent:
- 241:27eca07c9591
- Child:
- 243:30896263bd8b
diff -r 27eca07c9591 -r b235d67d25ba main.cpp --- 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)); // } // }