Bayley Wang
/
foc-ed_in_the_bot_compact
robot
errors.cpp
- Committer:
- bwang
- Date:
- 2018-11-11
- Revision:
- 240:2aaffa217627
- Parent:
- 235:fe49ec4e9c97
- Child:
- 241:eec17042a009
File content as of revision 240:2aaffa217627:
#include "errors.h" #include "globals.h" #include "prefs.h" int errors = 0, moded_errors = 0; int masks[32]; void init_masks() { masks[ERR_THROTTLE_DISABLED] = _MASK_MODE_RUN | _MASK_OP_TORQUE | _MASK_OP_DRIVING | _MASK_OP_SPEED | _MASK_OP_POSITION | _MASK_SRC_RC | _MASK_SRC_ANALOG; masks[ERR_POS_INVALID] = _MASK_MODE_RUN | _MASK_MODE_ZERO | _MASK_MODE_CHR | _MASK_OP_TORQUE | _MASK_OP_DRIVING | _MASK_OP_SPEED | _MASK_OP_POSITION | _MASK_SRC_RC | _MASK_SRC_ANALOG | _MASK_SRC_TERMINAL | _MASK_SRC_SERIAL | _MASK_SRC_CAN | _MASK_SRC_INTERNAL; masks[ERR_NOT_DRIVING] = _MASK_MODE_RUN | _MASK_OP_TORQUE | _MASK_OP_DRIVING | _MASK_OP_SPEED | _MASK_SRC_RC | _MASK_SRC_ANALOG | _MASK_SRC_TERMINAL | _MASK_SRC_SERIAL | _MASK_SRC_CAN | _MASK_SRC_INTERNAL; } void go_enabled() { control.enabled = true; io.en->write(1); } void go_disabled() { control.d_integral = 0.0f; control.q_integral = 0.0f; control.w_integral = 0.0f; control.enabled = false; io.en->write(0); } bool is_driving() { return control.user_cmd > 0.01f || control.torque_percent > 0.01f || fabsf(read.w) > _W_SAFE; } #define _seterr(x) errors |= (1 << x) #define _unseterr(x) errors &= ~(1 << x) #define _upderr(x) _unseterr(x) : _seterr(x) void check_errors_modeless() { io.throttle_in->get_enabled() ? _upderr(ERR_THROTTLE_DISABLED); io.pos->IsValid() ? _upderr(ERR_POS_INVALID); is_driving() ? _upderr(ERR_NOT_DRIVING); } bool checks_passed() { check_errors_modeless(); for (int k = 0; k < NUM_ERRORS; k++) { int mask_mode = masks[k] & _mask_mode(BREMS_mode); int mask_op = masks[k] & _mask_op(BREMS_op); int mask_src = masks[k] & _mask_src(BREMS_src); if ((errors & (1 << k)) != 0 && mask_mode != 0 && mask_op != 0 && mask_src != 0) { moded_errors |= 1 << k; } else { moded_errors &= ~(1 << k); } } if (moded_errors != 0) { return false; } return true; }