Bayley Wang
/
foc-ed_in_the_bot_compact
robot
errors.cpp@242:ac30f04fd6f7, 2018-11-12 (annotated)
- Committer:
- bwang
- Date:
- Mon Nov 12 10:58:59 2018 +0000
- Revision:
- 242:ac30f04fd6f7
- Parent:
- 241:eec17042a009
11/12/2018 05:56 - added check for user_cmd < 0 in OP_DRIVING in cmd_setp - this disables motor if the user enters a cmd < 0, without screwing with error logic.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bwang | 181:d3510c8beab6 | 1 | #include "errors.h" |
bwang | 187:523cf8c962e4 | 2 | #include "globals.h" |
bwang | 193:3abadeecf908 | 3 | #include "prefs.h" |
bwang | 181:d3510c8beab6 | 4 | |
bwang | 181:d3510c8beab6 | 5 | int errors = 0, moded_errors = 0; |
bwang | 181:d3510c8beab6 | 6 | int masks[32]; |
bwang | 181:d3510c8beab6 | 7 | |
bwang | 181:d3510c8beab6 | 8 | void init_masks() { |
bwang | 181:d3510c8beab6 | 9 | masks[ERR_THROTTLE_DISABLED] = _MASK_MODE_RUN | |
bwang | 181:d3510c8beab6 | 10 | _MASK_OP_TORQUE | _MASK_OP_DRIVING | _MASK_OP_SPEED | _MASK_OP_POSITION | |
bwang | 181:d3510c8beab6 | 11 | _MASK_SRC_RC | _MASK_SRC_ANALOG; |
bwang | 240:2aaffa217627 | 12 | masks[ERR_POS_INVALID] = _MASK_MODE_RUN | _MASK_MODE_ZERO | _MASK_MODE_CHR | |
bwang | 181:d3510c8beab6 | 13 | _MASK_OP_TORQUE | _MASK_OP_DRIVING | _MASK_OP_SPEED | _MASK_OP_POSITION | |
bwang | 181:d3510c8beab6 | 14 | _MASK_SRC_RC | _MASK_SRC_ANALOG | _MASK_SRC_TERMINAL | _MASK_SRC_SERIAL | |
bwang | 181:d3510c8beab6 | 15 | _MASK_SRC_CAN | _MASK_SRC_INTERNAL; |
bwang | 181:d3510c8beab6 | 16 | masks[ERR_NOT_DRIVING] = _MASK_MODE_RUN | |
bwang | 240:2aaffa217627 | 17 | _MASK_OP_TORQUE | _MASK_OP_DRIVING | _MASK_OP_SPEED | |
bwang | 181:d3510c8beab6 | 18 | _MASK_SRC_RC | _MASK_SRC_ANALOG | _MASK_SRC_TERMINAL | _MASK_SRC_SERIAL | |
bwang | 181:d3510c8beab6 | 19 | _MASK_SRC_CAN | _MASK_SRC_INTERNAL; |
bwang | 187:523cf8c962e4 | 20 | } |
bwang | 187:523cf8c962e4 | 21 | |
bwang | 187:523cf8c962e4 | 22 | void go_enabled() { |
bwang | 187:523cf8c962e4 | 23 | control.enabled = true; |
bwang | 187:523cf8c962e4 | 24 | io.en->write(1); |
bwang | 187:523cf8c962e4 | 25 | } |
bwang | 187:523cf8c962e4 | 26 | |
bwang | 187:523cf8c962e4 | 27 | void go_disabled() { |
bwang | 187:523cf8c962e4 | 28 | control.d_integral = 0.0f; |
bwang | 187:523cf8c962e4 | 29 | control.q_integral = 0.0f; |
bwang | 240:2aaffa217627 | 30 | control.w_integral = 0.0f; |
bwang | 187:523cf8c962e4 | 31 | control.enabled = false; |
bwang | 187:523cf8c962e4 | 32 | io.en->write(0); |
bwang | 187:523cf8c962e4 | 33 | } |
bwang | 187:523cf8c962e4 | 34 | |
bwang | 187:523cf8c962e4 | 35 | bool is_driving() { |
bwang | 242:ac30f04fd6f7 | 36 | return fabsf(control.user_cmd) > 0.01f || fabsf(control.torque_percent) > 0.01f || fabsf(read.w) > _W_SAFE; |
bwang | 193:3abadeecf908 | 37 | } |
bwang | 193:3abadeecf908 | 38 | |
bwang | 193:3abadeecf908 | 39 | #define _seterr(x) errors |= (1 << x) |
bwang | 193:3abadeecf908 | 40 | #define _unseterr(x) errors &= ~(1 << x) |
bwang | 193:3abadeecf908 | 41 | #define _upderr(x) _unseterr(x) : _seterr(x) |
bwang | 193:3abadeecf908 | 42 | void check_errors_modeless() { |
bwang | 193:3abadeecf908 | 43 | io.throttle_in->get_enabled() ? _upderr(ERR_THROTTLE_DISABLED); |
bwang | 193:3abadeecf908 | 44 | io.pos->IsValid() ? _upderr(ERR_POS_INVALID); |
bwang | 193:3abadeecf908 | 45 | is_driving() ? _upderr(ERR_NOT_DRIVING); |
bwang | 193:3abadeecf908 | 46 | } |
bwang | 193:3abadeecf908 | 47 | |
bwang | 193:3abadeecf908 | 48 | bool checks_passed() { |
bwang | 193:3abadeecf908 | 49 | check_errors_modeless(); |
bwang | 193:3abadeecf908 | 50 | for (int k = 0; k < NUM_ERRORS; k++) { |
bwang | 193:3abadeecf908 | 51 | int mask_mode = masks[k] & _mask_mode(BREMS_mode); |
bwang | 193:3abadeecf908 | 52 | int mask_op = masks[k] & _mask_op(BREMS_op); |
bwang | 193:3abadeecf908 | 53 | int mask_src = masks[k] & _mask_src(BREMS_src); |
bwang | 193:3abadeecf908 | 54 | if ((errors & (1 << k)) != 0 && mask_mode != 0 && mask_op != 0 && mask_src != 0) { |
bwang | 193:3abadeecf908 | 55 | moded_errors |= 1 << k; |
bwang | 193:3abadeecf908 | 56 | } else { |
bwang | 193:3abadeecf908 | 57 | moded_errors &= ~(1 << k); |
bwang | 193:3abadeecf908 | 58 | } |
bwang | 193:3abadeecf908 | 59 | } |
bwang | 194:05e119bc5a78 | 60 | |
bwang | 193:3abadeecf908 | 61 | if (moded_errors != 0) { |
bwang | 193:3abadeecf908 | 62 | return false; |
bwang | 241:eec17042a009 | 63 | } |
bwang | 241:eec17042a009 | 64 | return true; |
bwang | 181:d3510c8beab6 | 65 | } |