Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

errors.cpp@252:38644631ed97, 2018-11-13 (annotated)

Committer:
bwang
Date:
Tue Nov 13 17:46:23 2018 +0000
Revision:
252:38644631ed97
Parent:
242:ac30f04fd6f7 
11/13/2018 12:45 - hitting <return> on empty line prints "\r>", so that hitting enter after intially connecting to the controller generates a prompt

Who changed what in which revision?

UserRevisionLine numberNew 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 }