From e367d3b7d74895eab0212f1ff771dcb3b4280ffe Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Sun, 3 Mar 2019 19:01:46 -0800 Subject: [PATCH] Limit idle-current to 2A, Removed dangerous power-mode in favor of simpler enabled option, Fixed bug where motor driver could fail to disabled during estop --- CHANGELOG.md | 3 ++ src/avr/src/config.h | 1 + src/avr/src/drv8711.c | 16 +++++---- src/avr/src/drv8711.h | 1 - src/avr/src/main.c | 4 +-- src/avr/src/motor.c | 54 ++++++++---------------------- src/avr/src/stepper.c | 11 ------ src/avr/src/stepper.h | 1 - src/avr/src/vars.def | 2 +- src/py/bbctrl/Camera.py | 27 +++++++-------- src/py/bbctrl/Config.py | 6 ++++ src/py/bbctrl/Ctrl.py | 32 ++++++++++++++---- src/py/bbctrl/IOLoop.py | 4 +-- src/py/bbctrl/LCD.py | 3 +- src/py/bbctrl/Log.py | 9 +++-- src/py/bbctrl/Mach.py | 1 - src/py/bbctrl/Planner.py | 9 +++++ src/py/bbctrl/Pwr.py | 2 -- src/py/bbctrl/Web.py | 28 +++++++++------- src/resources/config-template.json | 13 ++++--- 20 files changed, 112 insertions(+), 115 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8325c6e..0f36593 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,9 @@ Buildbotics CNC Controller Firmware Changelog - Don't allow manual axis homing when soft limits are not set. - Right click to enable camera crosshair. - Demo mode. + - Limit idle-current to 2A. + - Removed dangerous ``power-mode`` in favor of simpler ``enabled`` option. + - Fixed bug where motor driver could fail to disabled during estop. ## v0.4.5 - Fix for random errors while running VFD. diff --git a/src/avr/src/config.h b/src/avr/src/config.h index 7c2c3e4..b25aefb 100644 --- a/src/avr/src/config.h +++ b/src/avr/src/config.h @@ -213,6 +213,7 @@ enum { #define CURRENT_SENSE_RESISTOR 0.05 // ohms #define CURRENT_SENSE_REF 2.75 // volts #define MAX_CURRENT 10 // amps +#define MAX_IDLE_CURRENT 2 // amps #define VELOCITY_MULTIPLIER 1000.0 #define ACCEL_MULTIPLIER 1000000.0 #define JERK_MULTIPLIER 1000000.0 diff --git a/src/avr/src/drv8711.c b/src/avr/src/drv8711.c index f440388..ad3893d 100644 --- a/src/avr/src/drv8711.c +++ b/src/avr/src/drv8711.c @@ -29,6 +29,7 @@ #include "status.h" #include "stepper.h" #include "switch.h" +#include "estop.h" #include #include @@ -165,6 +166,8 @@ static uint16_t _driver_get_isgain(int driver) { static uint8_t _driver_get_torque(int driver) { drv8711_driver_t *drv = &drivers[driver]; + if (estop_triggered()) return 0; + switch (drv->state) { case DRV8711_IDLE: return drv->idle.torque; case DRV8711_ACTIVE: return drv->drive.torque; @@ -198,7 +201,7 @@ static uint8_t _spi_next_command(uint8_t cmd) { // Next command if (++cmd == spi.ncmds) { cmd = 0; // Wrap around - st_enable(); // Enable motors + SET_PIN(MOTOR_ENABLE_PIN, !estop_triggered()); // Active high } // Prep next command @@ -352,6 +355,10 @@ void drv8711_init() { OUTSET_PIN(SPI_MOSI_PIN); // High DIRSET_PIN(SPI_MOSI_PIN); // Output + // Motor enable + OUTCLR_PIN(MOTOR_ENABLE_PIN); // Lo (disabled) + DIRSET_PIN(MOTOR_ENABLE_PIN); // Output + for (int i = 0; i < DRIVERS; i++) { uint8_t cs_pin = drivers[i].cs_pin; OUTSET_PIN(cs_pin); // High @@ -378,11 +385,6 @@ void drv8711_init() { } -void drv8711_shutdown() { - SPIC.INTCTRL = 0; // Disable SPI interrupts -} - - drv8711_state_t drv8711_get_state(int driver) { if (driver < 0 || DRIVERS <= driver) return DRV8711_DISABLED; return drivers[driver].state; @@ -432,7 +434,7 @@ float get_idle_current(int driver) { void set_idle_current(int driver, float value) { - if (driver < 0 || DRIVERS <= driver || value < 0 || MAX_CURRENT < value) + if (driver < 0 || DRIVERS <= driver || value < 0 || MAX_IDLE_CURRENT < value) return; _current_set(&drivers[driver].idle, value); diff --git a/src/avr/src/drv8711.h b/src/avr/src/drv8711.h index bbcb4ed..3119eb3 100644 --- a/src/avr/src/drv8711.h +++ b/src/avr/src/drv8711.h @@ -173,7 +173,6 @@ typedef void (*stall_callback_t)(int driver); void drv8711_init(); -void drv8711_shutdown(); drv8711_state_t drv8711_get_state(int driver); void drv8711_set_state(int driver, drv8711_state_t state); void drv8711_set_microsteps(int driver, uint16_t msteps); diff --git a/src/avr/src/main.c b/src/avr/src/main.c index d363ea2..1eaec24 100644 --- a/src/avr/src/main.c +++ b/src/avr/src/main.c @@ -67,16 +67,16 @@ int main(int argc, char *argv[]) { emu_init(); // Init emulator hw_init(); // hardware setup - must be first outputs_init(); // output pins + switch_init(); // switches + estop_init(); // emergency stop handler analog_init(); // analog input pins usart_init(); // serial port i2c_init(); // i2c port drv8711_init(); // motor drivers stepper_init(); // steppers motor_init(); // motors - switch_init(); // switches exec_init(); // motion exec vars_init(); // configuration variables - estop_init(); // emergency stop handler command_init(); // command queue sei(); // enable interrupts diff --git a/src/avr/src/motor.c b/src/avr/src/motor.c index 6a97bfb..41113d1 100644 --- a/src/avr/src/motor.c +++ b/src/avr/src/motor.c @@ -59,7 +59,7 @@ typedef struct { bool slave; uint16_t microsteps; // microsteps per full step bool reverse; - motor_power_mode_t power_mode; + bool enabled; float step_angle; // degrees per whole step float travel_rev; // mm or deg of travel per motor revolution float min_soft_limit; @@ -172,11 +172,7 @@ void motor_init() { } -bool motor_is_enabled(int motor) { - return motors[motor].power_mode != MOTOR_DISABLED; -} - - +bool motor_is_enabled(int motor) {return motors[motor].enabled;} int motor_get_axis(int motor) {return motors[motor].axis;} @@ -203,23 +199,12 @@ bool motor_get_homed(int motor) {return motors[motor].homed;} static void _update_power(int motor) { motor_t *m = &motors[motor]; - switch (m->power_mode) { - case MOTOR_POWERED_ONLY_WHEN_MOVING: - case MOTOR_POWERED_IN_CYCLE: - if (rtc_expired(m->power_timeout)) { - drv8711_set_state(motor, DRV8711_IDLE); - break; - } - // Fall through - - case MOTOR_ALWAYS_POWERED: + if (m->enabled) { + bool timedout = rtc_expired(m->power_timeout); // NOTE, we have ~5ms to enable the motor - drv8711_set_state(motor, DRV8711_ACTIVE); - break; + drv8711_set_state(motor, timedout ? DRV8711_IDLE : DRV8711_ACTIVE); - default: // Disabled - drv8711_set_state(motor, DRV8711_DISABLED); - } + } else drv8711_set_state(motor, DRV8711_DISABLED); } @@ -346,22 +331,13 @@ void motor_prep_move(int motor, float target) { m->timer_period = steps ? round(ticks_per_step) : 0; // Power motor - switch (m->power_mode) { - case MOTOR_POWERED_ONLY_WHEN_MOVING: - if (!m->timer_period) break; // Not moving - // Fall through - - case MOTOR_ALWAYS_POWERED: case MOTOR_POWERED_IN_CYCLE: - // Reset timeout - m->power_timeout = rtc_get_time() + MOTOR_IDLE_TIMEOUT * 1000; - break; - - default: // Disabled + if (!m->enabled) { m->timer_period = 0; m->encoder = m->commanded = m->position; m->error = 0; - break; - } + + } else if (m->timer_period) // Motor is moving so reset power timeout + m->power_timeout = rtc_get_time() + MOTOR_IDLE_TIMEOUT * 1000; _update_power(motor); // Queue move @@ -370,17 +346,15 @@ void motor_prep_move(int motor, float target) { // Var callbacks -uint8_t get_power_mode(int motor) {return motors[motor].power_mode;} +bool get_motor_enabled(int motor) {return motors[motor].enabled;} -void set_power_mode(int motor, uint8_t value) { +void set_motor_enabled(int motor, bool enabled) { if (motors[motor].slave) return; for (int m = motor; m < MOTORS; m++) if (motors[m].axis == motors[motor].axis) - motors[m].power_mode = - value <= MOTOR_POWERED_ONLY_WHEN_MOVING ? - (motor_power_mode_t)value : MOTOR_DISABLED; + motors[m].enabled = enabled; } @@ -452,7 +426,7 @@ void set_motor_axis(int motor, uint8_t axis) { set_step_angle(motor, motors[m].step_angle); set_travel(motor, motors[m].travel_rev); set_microstep(motor, motors[m].microsteps); - set_power_mode(motor, motors[m].power_mode); + set_motor_enabled(motor, motors[m].enabled); motors[motor].slave = true; break; } diff --git a/src/avr/src/stepper.c b/src/avr/src/stepper.c index ad277e8..2b6db21 100644 --- a/src/avr/src/stepper.c +++ b/src/avr/src/stepper.c @@ -62,10 +62,6 @@ static stepper_t st = {0}; void stepper_init() { - // Motor enable - OUTCLR_PIN(MOTOR_ENABLE_PIN); // Lo (disabled) - DIRSET_PIN(MOTOR_ENABLE_PIN); // Output - // Setup step timer TIMER_STEP.CTRLB = STEP_TIMER_WGMODE; // Waveform mode TIMER_STEP.INTCTRLA = STEP_TIMER_INTLVL; // Interrupt mode @@ -87,16 +83,9 @@ static void _load_move() { void st_shutdown() { - OUTCLR_PIN(MOTOR_ENABLE_PIN); // Disable motors TIMER_STEP.CTRLA = 0; // Stop stepper clock _end_move(); // Stop motor clocks ADCB_CH0_INTCTRL = 0; // Disable next move interrupt - drv8711_shutdown(); // Disable drivers -} - - -void st_enable() { - if (!estop_triggered()) OUTSET_PIN(MOTOR_ENABLE_PIN); // Active high } diff --git a/src/avr/src/stepper.h b/src/avr/src/stepper.h index 210c85a..d365e34 100644 --- a/src/avr/src/stepper.h +++ b/src/avr/src/stepper.h @@ -35,7 +35,6 @@ void stepper_init(); void st_shutdown(); -void st_enable(); bool st_is_busy(); void st_set_power_scale(float scale); void st_prep_power(const power_update_t powers[]); diff --git a/src/avr/src/vars.def b/src/avr/src/vars.def index 5e23021..191d6bd 100644 --- a/src/avr/src/vars.def +++ b/src/avr/src/vars.def @@ -36,7 +36,7 @@ // Motor VAR(motor_axis, an, u8, MOTORS, 1, 1) // Maps motor to axis -VAR(power_mode, pm, u8, MOTORS, 1, 1) // Motor power mode +VAR(motor_enabled, me, b8, MOTORS, 1, 1) // Motor enabled VAR(drive_current, dc, f32, MOTORS, 1, 1) // Max motor drive current VAR(idle_current, ic, f32, MOTORS, 1, 1) // Motor idle current diff --git a/src/py/bbctrl/Camera.py b/src/py/bbctrl/Camera.py index f8ab73b..97b83bf 100755 --- a/src/py/bbctrl/Camera.py +++ b/src/py/bbctrl/Camera.py @@ -274,14 +274,14 @@ class VideoDevice(object): class Camera(object): - def __init__(self, ctrl): - self.ctrl = ctrl - self.log = ctrl.log.get('Camera') + def __init__(self, ioloop, args, log): + self.ioloop = ioloop + self.log = log.get('Camera') - self.width = ctrl.args.width - self.height = ctrl.args.height - self.fps = ctrl.args.fps - self.fourcc = string_to_fourcc(ctrl.args.fourcc) + self.width = args.width + self.height = args.height + self.fps = args.fps + self.fourcc = string_to_fourcc(args.fourcc) self.offline_jpg = get_image_resource('http/images/offline.jpg') self.in_use_jpg = get_image_resource('http/images/in-use.jpg') @@ -300,8 +300,7 @@ class Camera(object): self.udevCtx = pyudev.Context() self.udevMon = pyudev.Monitor.from_netlink(self.udevCtx) self.udevMon.filter_by(subsystem = 'video4linux') - ctrl.ioloop.add_handler(self.udevMon, self._udev_handler, - ctrl.ioloop.READ) + ioloop.add_handler(self.udevMon, self._udev_handler, ioloop.READ) self.udevMon.start() @@ -336,7 +335,7 @@ class Camera(object): if isinstance(e, BlockingIOError): return self.log.warning('Failed to read from camera.') - self.ctrl.ioloop.remove_handler(fd) + self.ioloop.remove_handler(fd) self.close() return @@ -363,8 +362,8 @@ class Camera(object): self.dev.create_buffers(4) self.dev.start() - self.ctrl.ioloop.add_handler(self.dev, self._fd_handler, - self.ctrl.ioloop.READ) + self.ioloop.add_handler(self.dev, self._fd_handler, + self.ioloop.READ) self.log.info('Opened camera ' + path) @@ -386,7 +385,7 @@ class Camera(object): def close(self): if self.dev is None: return try: - self.ctrl.ioloop.remove_handler(self.dev) + self.ioloop.remove_handler(self.dev) try: self.dev.stop() except: pass @@ -427,7 +426,7 @@ class VideoHandler(web.RequestHandler): def __init__(self, app, request, **kwargs): super().__init__(app, request, **kwargs) - self.camera = app.get_ctrl(self.get_cookie('client-id')).camera + self.camera = app.camera @web.asynchronous diff --git a/src/py/bbctrl/Config.py b/src/py/bbctrl/Config.py index ad146c2..58a95d9 100644 --- a/src/py/bbctrl/Config.py +++ b/src/py/bbctrl/Config.py @@ -159,6 +159,12 @@ class Config(object): config['tool']['tool-reversed'] = reversed del config['tool']['spin-reversed'] + if version <= (0, 4, 6): + for motor in config['motors']: + if 2 < motor.get('idle-current', 0): motor['idle-current'] = 2 + if 'enabled' not in motor: + motor['enabled'] = motor.get('power-mode', '') != 'disabled' + config['version'] = self.version diff --git a/src/py/bbctrl/Ctrl.py b/src/py/bbctrl/Ctrl.py index 5e5a786..e6e536a 100644 --- a/src/py/bbctrl/Ctrl.py +++ b/src/py/bbctrl/Ctrl.py @@ -34,13 +34,20 @@ class Ctrl(object): self.args = args self.ioloop = bbctrl.IOLoop(ioloop) self.id = id + self.timeout = None if id and not os.path.exists(id): os.mkdir(id) - self.log = bbctrl.log.Log(self) + # Start log + if args.demo: log_path = self.get_path(filename = 'bbctrl.log') + else: log_path = args.log + self.log = bbctrl.log.Log(args, self.ioloop, log_path) + self.state = bbctrl.State(self) self.config = bbctrl.Config(self) + self.log.get('Ctrl').info('Starting %s' % self.id) + try: if args.demo: self.avr = bbctrl.AVREmu(self) else: self.avr = bbctrl.AVR(self) @@ -49,7 +56,7 @@ class Ctrl(object): self.lcd = bbctrl.LCD(self) self.mach = bbctrl.Mach(self, self.avr) self.preplanner = bbctrl.Preplanner(self) - self.jog = bbctrl.Jog(self) + if not args.demo: self.jog = bbctrl.Jog(self) self.pwr = bbctrl.Pwr(self) self.mach.connect() @@ -57,13 +64,23 @@ class Ctrl(object): self.lcd.add_new_page(bbctrl.MainLCDPage(self)) self.lcd.add_new_page(bbctrl.IPLCDPage(self.lcd)) - self.camera = None - if not args.disable_camera: - self.camera = bbctrl.Camera(self) - except Exception as e: self.log.get('Ctrl').exception(e) + def __del__(self): print('Ctrl deleted') + + + def clear_timeout(self): + if self.timeout is not None: self.ioloop.remove_timeout(self.timeout) + self.timeout = None + + + def set_timeout(self, cb, *args, **kwargs): + self.clear_timeout() + t = self.args.client_timeout + self.timeout = self.ioloop.call_later(t, cb, *args, **kwargs) + + def get_path(self, dir = None, filename = None): path = './' + self.id if self.id else '.' path = path if dir is None else (path + '/' + dir) @@ -89,6 +106,7 @@ class Ctrl(object): def close(self): - if not self.camera is None: self.camera.close() + self.log.get('Ctrl').info('Closing %s' % self.id) self.ioloop.close() self.avr.close() + self.mach.planner.close() diff --git a/src/py/bbctrl/IOLoop.py b/src/py/bbctrl/IOLoop.py index 641ab45..847c8d2 100644 --- a/src/py/bbctrl/IOLoop.py +++ b/src/py/bbctrl/IOLoop.py @@ -42,8 +42,8 @@ class IOLoop(object): def close(self): - for fd in self.fds: self.ioloop.remove_handler(fd) - for h in self.handles: self.ioloop.remove_timeout(h) + for fd in list(self.fds): self.ioloop.remove_handler(fd) + for h in list(self.handles): self.ioloop.remove_timeout(h) def add_handler(self, fd, handler, events): diff --git a/src/py/bbctrl/LCD.py b/src/py/bbctrl/LCD.py index 0d2cdcb..1f13c14 100644 --- a/src/py/bbctrl/LCD.py +++ b/src/py/bbctrl/LCD.py @@ -27,7 +27,6 @@ import lcd import atexit -from tornado.ioloop import PeriodicCallback class LCDPage: @@ -91,7 +90,7 @@ class LCD: self.set_message('Loading...') self._redraw(False) - atexit.register(self.goodbye) + if not ctrl.args.demo: atexit.register(self.goodbye) def set_message(self, msg): diff --git a/src/py/bbctrl/Log.py b/src/py/bbctrl/Log.py index c1a5205..cea8558 100644 --- a/src/py/bbctrl/Log.py +++ b/src/py/bbctrl/Log.py @@ -80,20 +80,19 @@ class Logger(object): class Log(object): - def __init__(self, ctrl): + def __init__(self, args, ioloop, path): + self.path = path self.listeners = [] self.loggers = {} - self.level = DEBUG if ctrl.args.verbose else INFO + self.level = DEBUG if args.verbose else INFO - if ctrl.args.demo: self.path = ctrl.get_path(filename = 'bbctrl.log') - else: self.path = ctrl.args.log self.f = None if self.path is None else open(self.path, 'w') # Log header version = pkg_resources.require('bbctrl')[0].version self._log('Log started v%s' % version) - self._log_time(ctrl.ioloop) + self._log_time(ioloop) def get_path(self): return self.path diff --git a/src/py/bbctrl/Mach.py b/src/py/bbctrl/Mach.py index 50b172a..dfbd9c9 100644 --- a/src/py/bbctrl/Mach.py +++ b/src/py/bbctrl/Mach.py @@ -28,7 +28,6 @@ import bbctrl from bbctrl.Comm import Comm import bbctrl.Cmd as Cmd -from tornado.ioloop import PeriodicCallback # Axis homing procedure: diff --git a/src/py/bbctrl/Planner.py b/src/py/bbctrl/Planner.py index 5d272b7..0cd7082 100644 --- a/src/py/bbctrl/Planner.py +++ b/src/py/bbctrl/Planner.py @@ -58,6 +58,7 @@ class Planner(): self.ctrl = ctrl self.log = ctrl.log.get('Planner') self.cmdq = CommandQueue(ctrl) + self.planner = None ctrl.state.add_listener(self._update) @@ -284,10 +285,18 @@ class Planner(): self.current_plan_time = 0 + def close(self): + # Release planner callbacks + if self.planner is not None: + self.planner.set_resolver(None) + self.planner.set_logger(None) + + def reset(self, stop = True): if stop: self.ctrl.mach.stop() self.planner = gplan.Planner() self.planner.set_resolver(self._get_var_cb) + # TODO logger is global and will not work correctly in demo mode self.planner.set_logger(self._log_cb, 1, 'LinePlanner:3') self.update_position() self.cmdq.clear() diff --git a/src/py/bbctrl/Pwr.py b/src/py/bbctrl/Pwr.py index 989a359..ded8aac 100644 --- a/src/py/bbctrl/Pwr.py +++ b/src/py/bbctrl/Pwr.py @@ -25,8 +25,6 @@ # # ################################################################################ -from tornado.ioloop import PeriodicCallback - import bbctrl diff --git a/src/py/bbctrl/Web.py b/src/py/bbctrl/Web.py index 199801e..820a7f4 100644 --- a/src/py/bbctrl/Web.py +++ b/src/py/bbctrl/Web.py @@ -414,6 +414,7 @@ class ClientConnection(object): self.ctrl.log.add_listener(self.send) self.is_open = True self.heartbeat() + self.app.opened(self.ctrl) def on_close(self): @@ -421,6 +422,7 @@ class ClientConnection(object): self.ctrl.state.remove_listener(self.send) self.ctrl.log.remove_listener(self.send) self.is_open = False + self.app.closed(self.ctrl) def on_message(self, data): self.ctrl.mach.mdi(data) @@ -478,6 +480,13 @@ class Web(tornado.web.Application): # Init controller if not self.args.demo: self.get_ctrl() + # Init camera + if not args.disable_camera: + if self.args.demo: log = bbctrl.log.Log(args, ioloop, 'camera.log') + else: log = self.get_ctrl().log + self.camera = bbctrl.Camera(ioloop, args, log) + + handlers = [ (r'/websocket', WSConnection), (r'/api/log', LogHandler), @@ -530,21 +539,18 @@ class Web(tornado.web.Application): print('Listening on http://%s:%d/' % (args.addr, args.port)) - self._reap_ctrls() + def opened(self, ctrl): ctrl.clear_timeout() - def _reap_ctrls(self): - if not self.args.demo: return - now = time.time() - for id in list(self.ctrls.keys()): - ctrl = self.ctrls[id] + def closed(self, ctrl): + # Time out clients in demo mode + if self.args.demo: ctrl.set_timeout(self._reap_ctrl, ctrl) - if ctrl.lastTS + self.args.client_timeout < now: - ctrl.close() - del self.ctrls[id] - self.ioloop.call_later(60, self._reap_ctrls) + def _reap_ctrl(self, ctrl): + ctrl.close() + del self.ctrls[ctrl.id] def get_ctrl(self, id = None): @@ -556,8 +562,6 @@ class Web(tornado.web.Application): else: ctrl = self.ctrls[id] - ctrl.lastTS = time.time() - return ctrl diff --git a/src/resources/config-template.json b/src/resources/config-template.json index a9a275b..766fa46 100644 --- a/src/resources/config-template.json +++ b/src/resources/config-template.json @@ -13,7 +13,7 @@ {"axis": "X"}, {"axis": "Y"}, {"axis": "Z"}, - {"axis": "A", "power-mode" : "disabled"} + {"axis": "A"} ], "template": { "general": { @@ -26,11 +26,10 @@ }, "power": { - "power-mode": { - "type": "enum", - "values": ["disabled", "always-on", "in-cycle", "when-moving"], - "default": "when-moving", - "code": "pm" + "enabled": { + "type": "bool", + "default": true, + "code": "me" }, "drive-current": { "type": "float", @@ -43,7 +42,7 @@ "idle-current": { "type": "float", "min": 0, - "max": 8, + "max": 2, "unit": "amps", "default": 0, "code": "ic" -- 2.27.0