From fb7f2568daba449719715673772a990f187cbdbb Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Tue, 16 Jan 2018 14:08:05 -0800 Subject: [PATCH] Added accel config, scale velocity and accel, connect planner seek function --- src/avr/src/axis.c | 29 ++++++++++++++++++---------- src/avr/src/config.h | 2 ++ src/avr/src/exec.c | 6 +++--- src/avr/src/seek.c | 3 +-- src/avr/src/switch.c | 1 + src/jade/templates/control-view.jade | 4 ++-- src/py/bbctrl/AVR.py | 3 ++- src/py/bbctrl/Cmd.py | 26 +++++++++++++++++++++++-- src/py/bbctrl/Planner.py | 10 ++++++---- src/resources/config-template.json | 13 ++++++++++--- 10 files changed, 70 insertions(+), 27 deletions(-) diff --git a/src/avr/src/axis.c b/src/avr/src/axis.c index 5f151d3..32ffa3a 100644 --- a/src/avr/src/axis.c +++ b/src/avr/src/axis.c @@ -124,8 +124,6 @@ float axis_get_vector_length(const float a[], const float b[]) { AXIS_SET(homed, bool) -AXIS_GET(velocity_max, float, 0) -AXIS_GET(accel_max, float, 0) AXIS_GET(homed, bool, false) AXIS_GET(homing_mode, homing_mode_t, HOMING_MANUAL) AXIS_GET(radius, float, 0) @@ -136,13 +134,24 @@ AXIS_GET(latch_velocity, float, 0) AXIS_GET(zero_backoff, float, 0) AXIS_GET(latch_backoff, float, 0) -/* Note on jerk functions - * - * Jerk values can be rather large. Jerk values are stored in the system in - * truncated format; values are divided by 1,000,000 then multiplied before use. - * - * The axis_jerk() functions expect the jerk in divided by 1,000,000 form. - */ + +/// Velocity is scaled by 1,000. +float axis_get_velocity_max(int axis) { + int motor = axis_get_motor(axis); + return motor == -1 ? 0 : axes[motor].velocity_max * VELOCITY_MULTIPLIER; +} +AXIS_VAR_GET(velocity_max, float) + + +/// Acceleration is scaled by 1,000. +float axis_get_accel_max(int axis) { + int motor = axis_get_motor(axis); + return motor == -1 ? 0 : axes[motor].accel_max * ACCEL_MULTIPLIER; +} +AXIS_VAR_GET(accel_max, float) + + +/// Jerk is scaled by 1,000,000. float axis_get_jerk_max(int axis) { int motor = axis_get_motor(axis); return motor == -1 ? 0 : axes[motor].jerk_max * JERK_MULTIPLIER; @@ -152,6 +161,7 @@ AXIS_VAR_GET(jerk_max, float) AXIS_VAR_SET(velocity_max, float) AXIS_VAR_SET(accel_max, float) +AXIS_VAR_SET(jerk_max, float) AXIS_VAR_SET(radius, float) AXIS_VAR_SET(travel_min, float) AXIS_VAR_SET(travel_max, float) @@ -160,7 +170,6 @@ AXIS_VAR_SET(search_velocity, float) AXIS_VAR_SET(latch_velocity, float) AXIS_VAR_SET(zero_backoff, float) AXIS_VAR_SET(latch_backoff, float) -AXIS_VAR_SET(jerk_max, float) float get_homing_dir(int axis) { diff --git a/src/avr/src/config.h b/src/avr/src/config.h index bb9ce42..0393dda 100644 --- a/src/avr/src/config.h +++ b/src/avr/src/config.h @@ -210,6 +210,8 @@ enum { #define CURRENT_SENSE_RESISTOR 0.05 // ohms #define CURRENT_SENSE_REF 2.75 // volts #define MAX_CURRENT 10 // amps +#define VELOCITY_MULTIPLIER 1000.0 +#define ACCEL_MULTIPLIER 1000.0 #define JERK_MULTIPLIER 1000000.0 #define SYNC_QUEUE_SIZE 4096 #define EXEC_FILL_TARGET 8 diff --git a/src/avr/src/exec.c b/src/avr/src/exec.c index 352a932..e8f75a3 100644 --- a/src/avr/src/exec.c +++ b/src/avr/src/exec.c @@ -110,9 +110,9 @@ stat_t exec_next() { // Variable callbacks int32_t get_line() {return ex.line;} uint8_t get_tool() {return ex.tool;} -float get_velocity() {return ex.velocity;} -float get_acceleration() {return ex.accel;} -float get_jerk() {return ex.jerk;} +float get_velocity() {return ex.velocity / VELOCITY_MULTIPLIER;} +float get_acceleration() {return ex.accel / ACCEL_MULTIPLIER;} +float get_jerk() {return ex.jerk / JERK_MULTIPLIER;} float get_feed_override() {return ex.feed_override;} float get_speed_override() {return ex.spindle_override;} float get_axis_position(int axis) {return ex.position[axis];} diff --git a/src/avr/src/seek.c b/src/avr/src/seek.c index 0f8ad1d..34516a1 100644 --- a/src/avr/src/seek.c +++ b/src/avr/src/seek.c @@ -31,6 +31,7 @@ #include "switch.h" #include "estop.h" #include "util.h" +#include "state.h" #include @@ -92,6 +93,4 @@ stat_t command_seek(char *cmd) { unsigned command_seek_size() {return sizeof(seek_t);} - - void command_seek_exec(void *data) {seek = *(seek_t *)data;} diff --git a/src/avr/src/switch.c b/src/avr/src/switch.c index 68898fc..28a2867 100644 --- a/src/avr/src/switch.c +++ b/src/avr/src/switch.c @@ -124,6 +124,7 @@ void switch_rtc_callback() { bool switch_is_active(int index) { + // NOTE, switch inputs are active lo switch (switches[index].type) { case SW_DISABLED: break; // A disabled switch cannot be active case SW_NORMALLY_OPEN: return !switches[index].state; diff --git a/src/jade/templates/control-view.jade b/src/jade/templates/control-view.jade index c1ae4a3..cacc72d 100644 --- a/src/jade/templates/control-view.jade +++ b/src/jade/templates/control-view.jade @@ -95,8 +95,8 @@ script#control-view-template(type="text/x-template") table.info tr th Velocity - td {{state.v || 0 | fixed 0}} - td mm/min + td {{state.v || 0 | fixed 2}} + td m/min tr th Line td {{0 <= state.ln ? state.ln : '-'}} diff --git a/src/py/bbctrl/AVR.py b/src/py/bbctrl/AVR.py index 96461c1..f96fdc8 100644 --- a/src/py/bbctrl/AVR.py +++ b/src/py/bbctrl/AVR.py @@ -3,6 +3,7 @@ import serial import json import time import logging +import traceback from collections import deque import bbctrl @@ -149,7 +150,7 @@ class AVR(): if self.ctrl.ioloop.READ & events: self.serial_read() if self.ctrl.ioloop.WRITE & events: self.serial_write() except Exception as e: - log.error('Serial handler error: %s', e) + log.error('Serial handler error: %s', traceback.format_exc()) def serial_write(self): diff --git a/src/py/bbctrl/Cmd.py b/src/py/bbctrl/Cmd.py index 51513ab..64d5148 100644 --- a/src/py/bbctrl/Cmd.py +++ b/src/py/bbctrl/Cmd.py @@ -18,8 +18,8 @@ FLUSH = 'F' STEP = 'S' RESUME = 'c' -SEEK_OPEN = 1 << 0 -SEEK_ERROR = 1 << 1 +SEEK_ACTIVE = 1 << 0 +SEEK_ERROR = 1 << 1 def encode_float(x): @@ -68,3 +68,25 @@ def speed(speed): return '#s=:' + encode_float(speed) def dwell(seconds): return 'd' + encode_float(seconds) def pause(optional = False): 'P' + ('1' if optional else '0') def jog(axes): return 'j' + encode_axes(axes) + + +def seek(switch, active, error): + cmd = SEEK + + if switch == 'probe': cmd += '1' + elif switch == 'x-min': cmd += '2' + elif switch == 'x-max': cmd += '3' + elif switch == 'y-min': cmd += '4' + elif switch == 'y-max': cmd += '5' + elif switch == 'z-min': cmd += '6' + elif switch == 'z-max': cmd += '7' + elif switch == 'a-min': cmd += '8' + elif switch == 'a-max': cmd += '9' + else: raise Exception('Unsupported switch "%s"' % switch) + + flags = 0 + if active: flags |= SEEK_ACTIVE + if error: flags |= SEEK_ERROR + cmd += chr(flags + ord('0')) + + return cmd diff --git a/src/py/bbctrl/Planner.py b/src/py/bbctrl/Planner.py index 6d71b7a..658ed8c 100644 --- a/src/py/bbctrl/Planner.py +++ b/src/py/bbctrl/Planner.py @@ -42,10 +42,10 @@ class Planner(): # Planner config self.config = { - "start": start, - "max-vel": get_vector('vm'), - "max-accel": get_vector('am'), - "max-jerk": get_vector('jm', 1000000), + "start": start, + "max-vel": get_vector('vm', 1000), + "max-accel": get_vector('am', 1000), + "max-jerk": get_vector('jm', 1000000), # TODO junction deviation & accel } log.info('Planner config: ' + json.dumps(self.config)) @@ -94,6 +94,8 @@ class Planner(): if type == 'speed': return Cmd.speed(block['speed']) if type == 'dwell': return Cmd.dwell(block['seconds']) if type == 'pause': return Cmd.pause(block['optional']) + if type == 'seek': + return Cmd.seek(block['switch'], block['active'], block['error']) raise Exception('Unknown planner type "%s"' % type) diff --git a/src/resources/config-template.json b/src/resources/config-template.json index cf47233..6f2d802 100644 --- a/src/resources/config-template.json +++ b/src/resources/config-template.json @@ -50,14 +50,21 @@ "max-velocity": { "type": "float", "min": 0, - "unit": "mm/min", - "default": 6000, + "unit": "m/min", + "default": 5, "code": "vm" }, + "max-accel": { + "type": "float", + "min": 0, + "unit": "m/min²", + "default": 100, + "code": "am" + }, "max-jerk": { "type": "float", "min": 0, - "unit": "mm/min³", + "unit": "km/min³", "default": 50, "code": "jm" }, -- 2.27.0