From: Joseph Coffland Date: Mon, 26 Feb 2018 17:16:42 +0000 (-0800) Subject: - Fixed: Config fails silently after web disconnect #112 X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=d78ff023e048a735494b5011baaf7012f96b3546;p=bbctrl-firmware - Fixed: Config fails silently after web disconnect #112 - Always reload the page after a disconnect. - Honor soft limits #111 (but not when jogging) - Limit switch going active while moving causes estop. #54 - Added more links to help page. - Fixed axis display on LCD. #122 - Added GCode cheat sheet. - Fixed LCD boot splash screen. #121 - Implemented tool change procedures and pause message box. #81 - Implemented program start and end procedures. --- diff --git a/CHANGELOG.md b/CHANGELOG.md index 832ed78..120a64b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,6 +1,18 @@ Buildbotics CNC Controller Firmware Change Log ============================================== +## v0.3.14 + - Fixed: Config fails silently after web disconnect #112 + - Always reload the page after a disconnect. + - Honor soft limits #111 (but not when jogging) + - Limit switch going active while moving causes estop. #54 + - Added more links to help page. + - Fixed axis display on LCD. #122 + - Added GCode cheat sheet. + - Fixed LCD boot splash screen. #121 + - Implemented tool change procedures and pause message box. #81 + - Implemented program start and end procedures. + ## v0.3.13 - Disable spindle and loads on stop. - Fixed several state transition (stop, pause, estop, etc.) problems. diff --git a/package.json b/package.json index 33e9411..b5c255d 100644 --- a/package.json +++ b/package.json @@ -1,6 +1,6 @@ { "name": "bbctrl", - "version": "0.3.13", + "version": "0.3.14", "homepage": "http://buildbotics.com/", "repository": "https://github.com/buildbotics/bbctrl-firmware", "license": "GPL-3.0+", diff --git a/src/avr/MoveLifecycle.md b/src/avr/MoveLifecycle.md deleted file mode 100644 index edb3101..0000000 --- a/src/avr/MoveLifecycle.md +++ /dev/null @@ -1,99 +0,0 @@ -# Notes on the lifecycle of a movement - -## Parsing -A move first starts off as a line of GCode which is parsed by -``gc_gcode_parser()`` which in turn calls ``_normalize_gcode_block()`` -and ``_parse_gcode_block()``. ``_parse_gcode_block()`` sets flags in -``mach.gf`` indicating which values have changed and the changed values in -``mach.gn``. ``_parse_gcode_block()`` then calls ``_execute_gcode_block()`` -which calls ``mach_*()`` functions which modify the state of the GCode machine. - -## Queuing -Some functions such as ``mach_straight_traverse()``, ``mach_straight_feed()`` -and ``mach_arc_feed()`` result in line moves being entered into the planner -queue. Others enter dwells or commands or into the queue. Planner buffer -entries encode everything needed to execute a move with out referring back to -the machine model. - -Line moves are entered into the queue by calls to ``mp_aline()``. Arcs are -converted to short straight line moves and are feed in as buffer room becomes -available, until complete, via calls to ``mach_arc_callback()`` which results in -multiple calls to ``mp_aline()``. - -``mp_aline()`` plans straight line movements by first calling -``mach_calc_move_time()`` which uses the current GCode state to estimate the -total time required to complete the move at the current feed rate and feed rate -mode. If the move time is long enough, a buffer is allocated. Its jerk, max -cruise velocity, max entry velocity, max delta velocity, max exit velocity and -breaking velocity are all computed. The move velocities are planned by a -call to ``mp_plan_block_list()``. Initially ``bf_func`` is set to -``mp_exec_aline()`` and the buffer is committed to the queue by calling -``mp_commit_write_buffer()``. - -## Planning -### Backward planning pass -``mp_plan_block_list()`` plans movements by first moving backwards through the -planning buffer until either the last entry is reached or a buffer marked not -``replannable`` is encountered. The ``breaking_velocity`` is propagated back -during the backwards pass. Next, begins the forward planning pass. - -### Forward planning pass -During the forward pass the entry velocity, cruise velocity and exit velocity -are computed and ``mp_calculate_trapezoid()`` is called to (re)compute the -velocity trapezoids of each buffer being considered. If a buffer's plan is -deemed optimal then it is marked not ``replannable`` to avoid replanning later. - -### Trapezoid planning -The call to ``mp_calculate_trapezoid()`` computes head, body and tail lengths -for a single planner buffer. Entry, cruse and exit velocities may be modified -to make the trapezoid fit with in the move length. Planning may result in a -degraded trapezoid. I.e. one with out all three sides. - -## Execution -The stepper motor driver interrupt routine calls ``mp_exec_move()`` to prepare -the next move for execution. ``mp_exec_move()`` access the next buffer in the -planner queue and calls the function pointed to by ``bf_func`` which is -initially set to ``mp_exec_aline()`` during planning. Each call to -``mp_exec_move()`` must prepare one and only one move fragment for the stepper -driver. The planner buffer is executed repeatedly as long as ``bf_func`` -returns ``STAT_EAGAIN``. - -### Move initialization -On the first call to ``mp_exec_aline()`` a call is made to -``_exec_aline_init()``. This function may stop processing the move if a -feedhold is in effect. It may also skip a move if it has zero length. -Otherwise, it initializes the move runtime state (``mr``) copying over the -variables set in the planner buffer. In addition, it computes waypoints at -the ends of each trapezoid section. Waypoints are used later to correct -position for rounding errors. - -### Move execution -After move initialization ``mp_exec_aline()`` calls ``_exec_aline_head()``, -``_exec_aline_body()`` and ``exec_aline_tail()`` on successive callbacks. -These functions are called repeatedly until each section finishes. If any -sections have zero length they are skipped and execution is passed immediately -to the next section. During each section, forward difference is used to map -the trapezoid computed during the planning stage to a fifth-degree Bezier -polynomial S-curve. The curve is used to find the appropriate velocity at the -next target position. - -``_exec_aline_segment()`` is called for each non-zero section to convert the -computed target position to target steps by calling ``mp_kinematics()``. The -move fragment is then passed to the stepper driver by a call to -``st_prep_line()``. When a segment is complete ``_exec_aline_segment()`` -returns ``STAT_OK`` indicating the next segment should be loaded. When all -non-zero segments have been executed, the move is complete. - -## Pulse generation -Calls to ``st_prep_line()`` prepare short (~5ms) move fragments for pulse -generation by the stepper driver. Move time in clock ticks is computed from -travel in steps and move duration. Then ``motor_prep_move()`` is called for -each motor. ``motor_prep_move()`` may perform step correction and enable the -motor. It then computes the optimal step clock divisor, clock ticks and sets -the move direction, taking the motor's configuration in to account. - -The stepper timer interrupt, after ending the previous move, calls -``motor_load_move()`` on each motor. This sets up and starts the motor clocks, -sets the motor direction lines and accumulates and resets the step encoders. -After (re)starting the motor clocks the interrupt signals a lower level -interrupt to call ``mp_exec_move()`` and load the next move fragment. diff --git a/src/avr/MoveLifecycleCalls.md b/src/avr/MoveLifecycleCalls.md deleted file mode 100644 index 2980675..0000000 --- a/src/avr/MoveLifecycleCalls.md +++ /dev/null @@ -1,77 +0,0 @@ -# Parsing, Queuing & Planning - * main() - * command_callback() - * gc_gcode_parser(const char *block) - * _normalize_gcode_block(...) - * _parse_gcode_block(const char *block) - * _execute_gcode_block() - * mach_*() - * mach_straight_traverse/feed() || mach_arc_feed() - * mp_aline(const float target[], float feed,. . .) - * mp_queue_get_tail() - * _calc_jerk*() - * _calc_move_time() - * _calc_max_velocities() - * _get_junction_vmax() - * mp_plan() - * mp_calculate_trapezoid() - * mp_get_target_length() - * mp_get_target_velocity() - * mp_queue_push(buffer_cb_t cb, uint32_t time) - -# Execution - * STEP_LOW_LEVEL_ISR - * mp_exec_move() - * mp_queue_get_head() - * mp_buffer->cb() - * _exec_dwell() - * mp_exec_aline() - * _exec_aline_init() - * _exec_aline_head() || _exec_aline_body() || _exec_aline_tail() - * _exec_aline_section() - * mp_runtime_move_to_target() - * mp_kinematics() - Converts target mm to steps and maps motors - * st_prep_line() - * motor_prep_move() - -# Step Output - * STEP_TIMER_ISR - * _load_move() - * _end_move() - * motor_end_move() - * _request_exec_move() - * Triggers STEP_LOW_LEVEL_ISR - * motor_load_move() - * motor_end_move() - - -# Data flow -## GCode block -char *line - -## Planner buffer - * mp_buffer_t pool[] - * float target[] - Target position in mm - * float unit[] - Direction vector - * float length, {head,body,tail}_length - Lengths in mm - * float {entry,cruise,exit,braking}_velocity - Target velocities in mm/min - * float {entry,cruise,exit,delta}_vmax - Max velocities in mm/min - * float jerk - Max jerk in km/min^3 - -``mach_*()`` functions compute ``target`` and call ``mp_aline()``. -``mp_aline()`` saves ``target`` computes ``unit`` and ``jerk``. Then it -estimates the move time in order to calculate max velocities. - -``mp_plan()`` and friends compute target velocities and trapezoid segment -lengths in two passes using max velocities in the current and neighboring -planner buffers. - -``mp_exec_aline()`` executes the trapezoidal move in the next planner buffer. - -## Move prep - * stepper_t st - * uint16_t seg_period - The step timer period - * motor_t motors[] - * uint8_t timer_clock - Clock divisor or zero for off - * uint16_t timer_period - Clock period - * direction_t direction - Step polarity diff --git a/src/avr/README.md b/src/avr/README.md index 24ba38a..97f5eb0 100644 --- a/src/avr/README.md +++ b/src/avr/README.md @@ -1,10 +1,6 @@ The Buildbotics firmware is a 4 axis motion control system designed for -high-performance on small to mid-sized machines. It was originally -derived from the [TinyG firmware](https://github.com/synthetos/TinyG). - -# Features - * 4 axis motion - * jerk controlled motion for acceleration planning (3rd order motion planning) +high-performance on small to mid-sized machines. It operates in conjunction +with the Buildbotics RaspberryPi firmware. # Build Instructions To build in Linux run: diff --git a/src/avr/src/analog.c b/src/avr/src/analog.c new file mode 100644 index 0000000..918f4fa --- /dev/null +++ b/src/avr/src/analog.c @@ -0,0 +1,90 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2018, Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "analog.h" + +#include "config.h" + +#include + +#include + + +typedef struct { + uint8_t pin; + uint16_t value; +} analog_port_t; + + +analog_port_t ports[] = { + {.pin = ANALOG_1_PIN}, + {.pin = ANALOG_2_PIN}, +}; + + +ISR(ADCA_CH0_vect) {ports[0].value = ADCA.CH0.RES;} +ISR(ADCA_CH1_vect) {ports[1].value = ADCA.CH1.RES;} + + +void analog_init() { + // Channel 0 + ADCA.CH0.CTRL = ADC_CH_GAIN_1X_gc | ADC_CH_INPUTMODE_SINGLEENDED_gc; + ADCA.CH0.MUXCTRL = ADC_CH_MUXPOS_PIN6_gc; + ADCA.CH0.INTCTRL = ADC_CH_INTLVL_LO_gc; + + // Channel 1 + ADCA.CH1.CTRL = ADC_CH_GAIN_1X_gc | ADC_CH_INPUTMODE_SINGLEENDED_gc; + ADCA.CH1.MUXCTRL = ADC_CH_MUXPOS_PIN7_gc; + ADCA.CH1.INTCTRL = ADC_CH_INTLVL_LO_gc; + + // ADC + ADCA.REFCTRL = ADC_REFSEL_INTVCC_gc; // 3.3V / 1.6 = 2.06V + ADCA.PRESCALER = ADC_PRESCALER_DIV512_gc; + ADCA.EVCTRL = ADC_SWEEP_01_gc; + ADCA.CTRLA = ADC_FLUSH_bm | ADC_ENABLE_bm; +} + + +float analog_get(unsigned port) { + if (1 < port) return 0; + return ports[port].value * (1.0 / 0x1000); +} + + +void analog_rtc_callback() { + static uint8_t count = 0; + + // Every 1/4 sec + if (++count == 250) { + count = 0; + ADCA.CTRLA |= ADC_CH0START_bm | ADC_CH1START_bm; + } +} + + +// Var callbacks +float get_analog_input(int port) {return analog_get(port);} diff --git a/src/avr/src/analog.h b/src/avr/src/analog.h new file mode 100644 index 0000000..6aaf592 --- /dev/null +++ b/src/avr/src/analog.h @@ -0,0 +1,33 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2018, Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + + +void analog_init(); +float analog_get(unsigned port); +void analog_rtc_callback(); diff --git a/src/avr/src/axis.c b/src/avr/src/axis.c index 5248978..ee9da55 100644 --- a/src/avr/src/axis.c +++ b/src/avr/src/axis.c @@ -70,6 +70,17 @@ int axis_get_id(char axis) { int axis_get_motor(int axis) {return motor_map[axis];} +bool axis_get_homed(int axis) { + return axis_is_enabled(axis) ? motor_get_homed(axis_get_motor(axis)) : false; +} + + +float axis_get_soft_limit(int axis, bool min) { + if (!axis_is_enabled(axis)) return min ? -INFINITY : INFINITY; + return motor_get_soft_limit(axis_get_motor(axis), min); +} + + // Map axes to first matching motor void axis_map_motors() { for (int axis = 0; axis < AXES; axis++) diff --git a/src/avr/src/axis.h b/src/avr/src/axis.h index 706d072..e825a93 100644 --- a/src/avr/src/axis.h +++ b/src/avr/src/axis.h @@ -42,6 +42,8 @@ bool axis_is_enabled(int axis); char axis_get_char(int axis); int axis_get_id(char axis); int axis_get_motor(int axis); +bool axis_get_homed(int axis); +float axis_get_soft_limit(int axis, bool min); void axis_map_motors(); float axis_get_vector_length(const float a[], const float b[]); diff --git a/src/avr/src/command.c b/src/avr/src/command.c index 4041f1c..d9e7780 100644 --- a/src/avr/src/command.c +++ b/src/avr/src/command.c @@ -189,7 +189,6 @@ bool command_callback() { if (!block) block = usart_readline(); if (!block) return false; // No command - cmd.active = true; // Disables LCD booting message stat_t status = STAT_OK; // Special processing for synchronous commands @@ -201,7 +200,10 @@ bool command_callback() { } // Dispatch non-empty commands - if (*block && status == STAT_OK) status = _dispatch(block); + if (*block && status == STAT_OK) { + status = _dispatch(block); + if (status == STAT_OK) cmd.active = true; // Disables LCD booting message + } block = 0; // Command consumed diff --git a/src/avr/src/command.def b/src/avr/src/command.def index 0cea17b..680990d 100644 --- a/src/avr/src/command.def +++ b/src/avr/src/command.def @@ -30,6 +30,7 @@ CMD('#', sync_var, 1, "Set variable synchronous") CMD('s', seek, 1, "[switch][flags:active|error]") CMD('a', set_axis, 1, "[axis][position] Set axis position") CMD('l', line, 1, "[targetVel][maxJerk][axes][times]") +CMD('I', input, 1, "[a|d][port][mode][timeout] Read input") CMD('d', dwell, 1, "[seconds]") CMD('P', pause, 1, "[type] Pause control") CMD('S', stop, 0, "Stop move, spindle and load outputs") diff --git a/src/avr/src/config.h b/src/avr/src/config.h index edc2584..fbee04d 100644 --- a/src/avr/src/config.h +++ b/src/avr/src/config.h @@ -97,6 +97,7 @@ enum { #define AXES 6 // number of axes #define MOTORS 4 // number of motors on the board #define OUTS 5 // number of supported pin outputs +#define ANALOG 2 // number of supported analog inputs // Switch settings. See switch.c diff --git a/src/avr/src/exec.c b/src/avr/src/exec.c index 97edf58..8869734 100644 --- a/src/avr/src/exec.c +++ b/src/avr/src/exec.c @@ -33,6 +33,9 @@ #include "util.h" #include "command.h" #include "report.h" +#include "switch.h" +#include "seek.h" +#include "estop.h" #include "config.h" @@ -49,11 +52,21 @@ static struct { } ex; +static void _limit_switch_cb(switch_id_t sw, bool active) { + if (sw == seek_get_switch()) return; + if (ex.velocity && active) estop_trigger(STAT_ESTOP_SWITCH); +} + + void exec_init() { memset(&ex, 0, sizeof(ex)); ex.feed_override = 1; ex.spindle_override = 1; // TODO implement overrides + + // Set callback for limit switches + for (switch_id_t sw = SW_MIN_X; sw <= SW_MAX_A; sw++) + switch_set_callback(sw, _limit_switch_cb); } diff --git a/src/avr/src/hardware.c b/src/avr/src/hardware.c index af919d7..6f67d56 100644 --- a/src/avr/src/hardware.c +++ b/src/avr/src/hardware.c @@ -146,4 +146,22 @@ void hw_reset_handler() { } +uint8_t hw_disable_watchdog() { + uint8_t state = WDT.CTRL; + cli(); + CCP = CCP_IOREG_gc; + WDT.CTRL = WDT_CEN_bm; + sei(); + return state; +} + + +void hw_restore_watchdog(uint8_t state) { + cli(); + CCP = CCP_IOREG_gc; + WDT.CTRL = state | WDT_CEN_bm; + sei(); +} + + const char *get_hw_id() {return hw.id;} diff --git a/src/avr/src/hardware.h b/src/avr/src/hardware.h index 27407cb..3bdef89 100644 --- a/src/avr/src/hardware.h +++ b/src/avr/src/hardware.h @@ -36,3 +36,6 @@ void hardware_init(); void hw_request_hard_reset(); void hw_hard_reset(); void hw_reset_handler(); + +uint8_t hw_disable_watchdog(); +void hw_restore_watchdog(uint8_t state); diff --git a/src/avr/src/io.c b/src/avr/src/io.c new file mode 100644 index 0000000..e581efb --- /dev/null +++ b/src/avr/src/io.c @@ -0,0 +1,120 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2018, Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "io.h" + +#include "status.h" +#include "util.h" +#include "command.h" +#include "exec.h" +#include "rtc.h" +#include "analog.h" + +#include +#include +#include + + +typedef struct { + int8_t port; + bool digital; + input_mode_t mode; + float timeout; +} input_cmd_t; + + +static input_cmd_t active_cmd = {-1,}; +static uint32_t timeout; + + +void io_rtc_callback() { + if (active_cmd.port == -1) return; + + bool done = false; + float result = 0; + if (active_cmd.mode == INPUT_IMMEDIATE || + rtc_expired(active_cmd.timeout)) done = true; + + // TODO handle modes + + if (done) { + if (active_cmd.digital) { // TODO + } else result = analog_get(active_cmd.port); + + // TODO find a better way to send this + printf("{\"result\": %f}\n", result); + active_cmd.port = -1; + } +} + + +static stat_t _exec_cb() { + if (active_cmd.port == -1) exec_set_cb(0); + return STAT_NOP; +} + + +// Command callbacks +stat_t command_input(char *cmd) { + input_cmd_t input_cmd; + cmd++; // Skip command + + // Analog or digital + if (cmd[0] == 'd') input_cmd.digital = true; + else if (cmd[0] == 'a') input_cmd.digital = false; + else return STAT_INVALID_ARGUMENTS; + cmd++; + + // Port index + if (!isdigit(cmd[1])) return STAT_INVALID_ARGUMENTS; + input_cmd.port = cmd[1] - '0'; + cmd++; + + // Mode + if (!isdigit(cmd[2])) return STAT_INVALID_ARGUMENTS; + input_cmd.mode = cmd[2] - '0'; + if (INPUT_LOW < input_cmd.mode) return STAT_INVALID_ARGUMENTS; + cmd++; + + // Timeout + if (!decode_float(&cmd, &input_cmd.timeout)) return STAT_BAD_FLOAT; + + command_push(COMMAND_input, &input_cmd); + + return STAT_OK; +} + + +unsigned command_input_size() {return sizeof(input_cmd_t);} + + +void command_input_exec(void *data) { + active_cmd = *(input_cmd_t *)data; + + timeout = rtc_get_time() + active_cmd.timeout * 1000; + exec_set_cb(_exec_cb); +} diff --git a/src/avr/src/io.h b/src/avr/src/io.h new file mode 100644 index 0000000..f3f2df5 --- /dev/null +++ b/src/avr/src/io.h @@ -0,0 +1,40 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2018, Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + + +typedef enum { + INPUT_IMMEDIATE, + INPUT_RISE, + INPUT_FALL, + INPUT_HIGH, + INPUT_LOW, +} input_mode_t; + + +void io_rtc_callback(); diff --git a/src/avr/src/jog.c b/src/avr/src/jog.c index ed1cd4c..b52452b 100644 --- a/src/avr/src/jog.c +++ b/src/avr/src/jog.c @@ -112,64 +112,79 @@ static float _compute_deccel_dist(float vel, float accel, float jerk) { return dist; } +#else // Analytical version static float _compute_deccel_dist(float vel, float accel, float jerk) { + // TODO Fix this function + float dist = 0; + float t = accel / jerk; // Compute distance to decrease accel to zero if (0 < accel) { - // s(a/j) = v * a / j + 2 * a^3 / (3 * j^2) - dist += vel * accel / jerk + 2 * accel * accel * accel / (3 * jerk * jerk); - // v(a/j) = a^2 / 2j + v - vel += accel * accel / (2 * jerk); + // s(t) = v * t + 2/3 * a * t^2 + dist += vel * t + 2.0 / 3.0 * accel * t * t; + + // v(t) = a * t / 2 + v + vel += accel * t / 2; accel = 0; } // Compute max deccel given accel, vel and jerk - float maxDeccel = -sqrt(0.5 * accel * accel + vel * jerk); + float maxDeccel = -sqrt(0.5 * square(accel) + vel * jerk); // Compute distance to max deccel if (maxDeccel < accel) { float t = (maxDeccel - accel) / -jerk; - dist += vel * t + accel * t * t / 2 - jerk * t * t * t / 6; - vel += accel * t - jerk * t * t / 2; + dist += scurve_distance(t, vel, accel, -jerk); + vel += scurve_velocity(t, accel, -jerk); accel = maxDeccel; } // Compute distance to zero vel - float t = -accel / jerk; - dist += vel * t + accel * t * t / 2 + jerk * t * t * t / 6; + dist += scurve_distance(t, vel, accel, jerk); return dist; } #endif -#if 0 -static bool _soft_limit(int axis, float V, float A) { - jog_axis_t *a = &jr.axes[axis]; +static float _soft_limit(int axis, float V, float Vt, float A) { + // Get travel direction + float dir = jr.axes[axis].velocity; + if (!dir) dir = jr.axes[axis].target; + if (!dir) return 0; // Check if axis is homed - if (!axis_get_homed(axis)) return false; + if (!axis_get_homed(axis)) return Vt; // Check if limits are enabled - float min = axis_get_travel_min(axis); - float max = axis_get_travel_max(axis); - if (min == max) return false; + float min = axis_get_soft_limit(axis, true); + float max = axis_get_soft_limit(axis, false); + if (min == max) return Vt; - // Check if we need to stop to avoid exceeding a limit + // Move allowed if at or past limit but headed out + // Move not allowed if at or past limit and heading further in + float position = exec_get_axis_position(axis); + if (position <= min) return 0 < dir ? Vt : 0; + if (max <= position) return dir < 0 ? Vt : 0; + + // Compute dist to decel float jerk = axis_get_jerk_max(axis); float deccelDist = _compute_deccel_dist(V, A, jerk); - float position = exec_get_axis_position(axis); - if (a->velocity < 0 && position <= min + deccelDist) return true; - if (0 < a->velocity && max - deccelDist <= position) return true; + // Check if decell distance will lead to exceeding a limit + if (0 < dir && position <= min + deccelDist) return 0; + if (dir < 0 && max - deccelDist <= position) return 0; - return false; + // Check if decell distance will lead near limit + if (0 < dir && position <= min + deccelDist + 5) return MIN_VELOCITY; + if (dir < 0 && max - deccelDist - 5 <= position) return MIN_VELOCITY; + + return Vt; } -#endif static float _compute_axis_velocity(int axis) { @@ -179,7 +194,7 @@ static float _compute_axis_velocity(int axis) { float Vt = fabs(a->target); // Apply soft limits - //if (_soft_limit(axis, V, a->accel)) Vt = MIN_VELOCITY; + Vt = _soft_limit(axis, V, Vt, a->accel); // Check if velocity has reached its target if (fp_EQ(V, Vt)) { diff --git a/src/avr/src/lcd.c b/src/avr/src/lcd.c index 71f6be8..2dbabf4 100644 --- a/src/avr/src/lcd.c +++ b/src/avr/src/lcd.c @@ -27,9 +27,11 @@ #include "lcd.h" #include "rtc.h" +#include "hardware.h" #include "command.h" #include +#include #include #include @@ -37,10 +39,10 @@ void lcd_init(uint8_t addr) { // Enable I2C master - TWIC.MASTER.BAUD = 0x9b; // 100 KHz with 32MHz clock + TWIC.MASTER.BAUD = F_CPU / 2 / 100000 - 5; // 100 KHz TWIC.MASTER.CTRLA = TWI_MASTER_ENABLE_bm; TWIC.MASTER.CTRLB = TWI_MASTER_TIMEOUT_DISABLED_gc; - TWIC.MASTER.STATUS |= 1; // Force idle + TWIC.MASTER.STATUS = TWI_MASTER_BUSSTATE_IDLE_gc; _delay_ms(50); lcd_nibble(addr, 3 << 4); // Home @@ -120,12 +122,15 @@ void _splash(uint8_t addr) { void lcd_splash() { + wdt_disable(); _splash(0x27); _splash(0x3f); + wdt_enable(WDTO_250MS); } void lcd_rtc_callback() { + // Display the splash if we haven't gotten any commands in 1sec since boot if (!command_is_active() && rtc_get_time() == 1000) lcd_splash(); } diff --git a/src/avr/src/main.c b/src/avr/src/main.c index 7fcc497..377952c 100644 --- a/src/avr/src/main.c +++ b/src/avr/src/main.c @@ -40,7 +40,7 @@ #include "i2c.h" #include "pgmspace.h" #include "outputs.h" -#include "lcd.h" +#include "analog.h" #include "exec.h" #include "state.h" @@ -58,6 +58,7 @@ int main() { hardware_init(); // hardware setup - must be first outputs_init(); // output pins + analog_init(); // analog input pins usart_init(); // serial port i2c_init(); // i2c port drv8711_init(); // motor drivers @@ -68,7 +69,7 @@ int main() { exec_init(); // motion exec vars_init(); // configuration variables estop_init(); // emergency stop handler - command_init(); + command_init(); // command queue sei(); // enable interrupts diff --git a/src/avr/src/motor.c b/src/avr/src/motor.c index 3484a56..81cf475 100644 --- a/src/avr/src/motor.c +++ b/src/avr/src/motor.c @@ -56,6 +56,9 @@ typedef struct { motor_power_mode_t power_mode; float step_angle; // degrees per whole step float travel_rev; // mm or deg of travel per motor revolution + float min_soft_limit; + float max_soft_limit; + bool homed; uint8_t step_pin; uint8_t dir_pin; TC0_t *timer; @@ -134,6 +137,10 @@ void motor_init() { for (int motor = 0; motor < MOTORS; motor++) { motor_t *m = &motors[motor]; + // Default soft limits + m->min_soft_limit = -INFINITY; + m->max_soft_limit = INFINITY; + _update_config(motor); // IO pins @@ -187,6 +194,14 @@ void motor_set_position(int motor, float position) { } +float motor_get_soft_limit(int motor, bool min) { + return min ? motors[motor].min_soft_limit : motors[motor].max_soft_limit; +} + + +bool motor_get_homed(int motor) {return motors[motor].homed;} + + static void _update_power(int motor) { motor_t *m = &motors[motor]; @@ -347,6 +362,19 @@ void motor_prep_move(int motor, float time, float target) { // Var callbacks +uint8_t get_power_mode(int motor) {return motors[motor].power_mode;} + + +void set_power_mode(int motor, uint8_t value) { + 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 ? value : MOTOR_DISABLED; +} + + float get_step_angle(int motor) {return motors[motor].step_angle;} @@ -379,8 +407,6 @@ uint16_t get_microstep(int motor) {return motors[motor].microsteps;} void set_microstep(int motor, uint16_t value) { - if (motor < 0 || MOTORS <= motor) return; - switch (value) { case 1: case 2: case 4: case 8: case 16: case 32: case 64: case 128: case 256: break; @@ -398,28 +424,6 @@ void set_microstep(int motor, uint16_t value) { } -bool get_reverse(int motor) { - if (motor < 0 || MOTORS <= motor) return 0; - return motors[motor].reverse; -} - - -void set_reverse(int motor, bool value) {motors[motor].reverse = value;} - - -uint8_t get_power_mode(int motor) {return motors[motor].power_mode;} - - -void set_power_mode(int motor, uint8_t value) { - 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 ? value : MOTOR_DISABLED; -} - - char get_motor_axis(int motor) {return motors[motor].axis;} @@ -446,5 +450,27 @@ void set_motor_axis(int motor, uint8_t axis) { } +bool get_reverse(int motor) {return motors[motor].reverse;} +void set_reverse(int motor, bool value) {motors[motor].reverse = value;} + + +float get_min_soft_limit(int motor) {return motors[motor].min_soft_limit;} +float get_max_soft_limit(int motor) {return motors[motor].max_soft_limit;} + + +void set_min_soft_limit(int motor, float limit) { + motors[motor].min_soft_limit = limit; +} + + +void set_max_soft_limit(int motor, float limit) { + motors[motor].max_soft_limit = limit; +} + + +bool get_homed(int motor) {return motors[motor].homed;} +void set_homed(int motor, bool homed) {motors[motor].homed = homed;} + + int32_t get_encoder(int m) {return motors[m].encoder;} int32_t get_error(int m) {return motors[m].error;} diff --git a/src/avr/src/motor.h b/src/avr/src/motor.h index c83d774..bcfffd8 100644 --- a/src/avr/src/motor.h +++ b/src/avr/src/motor.h @@ -47,6 +47,8 @@ void motor_init(); bool motor_is_enabled(int motor); int motor_get_axis(int motor); void motor_set_position(int motor, float position); +float motor_get_soft_limit(int motor, bool min); +bool motor_get_homed(int motor); stat_t motor_rtc_callback(); diff --git a/src/avr/src/outputs.h b/src/avr/src/outputs.h index 0e5a6f8..086f480 100644 --- a/src/avr/src/outputs.h +++ b/src/avr/src/outputs.h @@ -38,7 +38,7 @@ typedef enum { } output_state_t; -// OUT_inactive_active +/// OUT__ typedef enum { OUT_DISABLED, OUT_LO_HI, diff --git a/src/avr/src/rtc.c b/src/avr/src/rtc.c index edf9779..702355a 100644 --- a/src/avr/src/rtc.c +++ b/src/avr/src/rtc.c @@ -28,6 +28,8 @@ #include "rtc.h" #include "switch.h" +#include "analog.h" +#include "io.h" #include "huanyang.h" #include "motor.h" #include "lcd.h" @@ -45,10 +47,12 @@ static uint32_t ticks; ISR(RTC_OVF_vect) { ticks++; + lcd_rtc_callback(); switch_rtc_callback(); + analog_rtc_callback(); + io_rtc_callback(); hy_rtc_callback(); if (!(ticks & 255)) motor_rtc_callback(); - lcd_rtc_callback(); wdt_reset(); } diff --git a/src/avr/src/seek.c b/src/avr/src/seek.c index 43b235a..a6e7d6b 100644 --- a/src/avr/src/seek.c +++ b/src/avr/src/seek.c @@ -44,16 +44,20 @@ enum { typedef struct { - int8_t sw; + bool active; + switch_id_t sw; uint8_t flags; } seek_t; -static seek_t seek = {-1, 0}; +static seek_t seek = {false, -1, 0}; + + +switch_id_t seek_get_switch() {return seek.active ? seek.sw : -1;} bool seek_switch_found() { - if (seek.sw <= 0) return false; + if (!seek.active) return false; bool inactive = !(seek.flags & SEEK_ACTIVE); @@ -67,12 +71,12 @@ bool seek_switch_found() { void seek_end() { - if (seek.sw <= 0) return; + if (!seek.active) return; if (!(SEEK_FOUND & seek.flags) && (SEEK_ERROR & seek.flags)) estop_trigger(STAT_SEEK_NOT_FOUND); - seek.sw = -1; + seek.active = false; } @@ -85,7 +89,7 @@ stat_t command_seek(char *cmd) { int8_t flags = decode_hex_nibble(cmd[2]); if (flags & 0xfc) return STAT_INVALID_ARGUMENTS; - seek_t seek = {sw, flags}; + seek_t seek = {true, sw, flags}; command_push(*cmd, &seek); return STAT_OK; diff --git a/src/avr/src/seek.h b/src/avr/src/seek.h index 7e97dd7..6c6fa53 100644 --- a/src/avr/src/seek.h +++ b/src/avr/src/seek.h @@ -27,8 +27,11 @@ #pragma once +#include "switch.h" + #include +switch_id_t seek_get_switch(); bool seek_switch_found(); void seek_end(); diff --git a/src/avr/src/state.c b/src/avr/src/state.c index 73b55a1..f0296ac 100644 --- a/src/avr/src/state.c +++ b/src/avr/src/state.c @@ -120,6 +120,12 @@ void state_seek_hold() { static void _stop() { switch (state_get()) { case STATE_STOPPING: + if (!exec_get_velocity()) { + _set_state(STATE_READY); + _stop(); + return; + } + case STATE_RUNNING: _set_hold_reason(HOLD_REASON_USER_STOP); _set_state(STATE_STOPPING); diff --git a/src/avr/src/switch.c b/src/avr/src/switch.c index 6da31c7..aac99e2 100644 --- a/src/avr/src/switch.c +++ b/src/avr/src/switch.c @@ -28,8 +28,6 @@ #include "switch.h" #include "config.h" -#include - #include @@ -63,7 +61,7 @@ static switch_t switches[] = { }; -const int num_switches = sizeof(switches) / sizeof (switch_t); +static const int num_switches = sizeof(switches) / sizeof (switch_t); void switch_init() { @@ -94,43 +92,44 @@ void switch_rtc_callback() { } -bool switch_is_active(int index) { +bool switch_is_active(switch_id_t sw) { + if (sw < 0 || num_switches <= sw) return false; + // NOTE, switch inputs are active lo - switch (switches[index].type) { + switch (switches[sw].type) { case SW_DISABLED: break; // A disabled switch cannot be active - case SW_NORMALLY_OPEN: return !switches[index].state; - case SW_NORMALLY_CLOSED: return switches[index].state; + case SW_NORMALLY_OPEN: return !switches[sw].state; + case SW_NORMALLY_CLOSED: return switches[sw].state; } return false; } -bool switch_is_enabled(int index) { - return switch_get_type(index) != SW_DISABLED; +bool switch_is_enabled(switch_id_t sw) { + return switch_get_type(sw) != SW_DISABLED; } -switch_type_t switch_get_type(int index) { - return - (index < 0 || num_switches <= index) ? SW_DISABLED : switches[index].type; +switch_type_t switch_get_type(switch_id_t sw) { + return (sw < 0 || num_switches <= sw) ? SW_DISABLED : switches[sw].type; } -void switch_set_type(int index, switch_type_t type) { - if (index < 0 || num_switches <= index) return; - switch_t *s = &switches[index]; +void switch_set_type(switch_id_t sw, switch_type_t type) { + if (sw < 0 || num_switches <= sw) return; + switch_t *s = &switches[sw]; if (s->type != type) { - bool wasActive = switch_is_active(index); + bool wasActive = switch_is_active(sw); s->type = type; - bool isActive = switch_is_active(index); - if (wasActive != isActive && s->cb) s->cb(index, isActive); + bool isActive = switch_is_active(sw); + if (wasActive != isActive && s->cb) s->cb(sw, isActive); } } -void switch_set_callback(int index, switch_callback_t cb) { - switches[index].cb = cb; +void switch_set_callback(switch_id_t sw, switch_callback_t cb) { + switches[sw].cb = cb; } diff --git a/src/avr/src/switch.h b/src/avr/src/switch.h index d1dcdd3..ef2a459 100644 --- a/src/avr/src/switch.h +++ b/src/avr/src/switch.h @@ -64,8 +64,8 @@ typedef void (*switch_callback_t)(switch_id_t sw, bool active); void switch_init(); void switch_rtc_callback(); -bool switch_is_active(int index); -bool switch_is_enabled(int index); -switch_type_t switch_get_type(int index); -void switch_set_type(int index, switch_type_t type); -void switch_set_callback(int index, switch_callback_t cb); +bool switch_is_active(switch_id_t sw); +bool switch_is_enabled(switch_id_t sw); +switch_type_t switch_get_type(switch_id_t sw); +void switch_set_type(switch_id_t sw, switch_type_t type); +void switch_set_callback(switch_id_t sw, switch_callback_t cb); diff --git a/src/avr/src/vars.def b/src/avr/src/vars.def index fb85983..7fb8edb 100644 --- a/src/avr/src/vars.def +++ b/src/avr/src/vars.def @@ -25,22 +25,32 @@ \******************************************************************************/ -#define AXES_LABEL "xyzabc" +#define AXES_LABEL "xyzabc" #define MOTORS_LABEL "0123" -#define OUTS_LABEL "ed12f" +#define OUTS_LABEL "ed12f" +#define ANALOG_LABEL "12" // VAR(name, code, type, index, settable, report, help) // Motor VAR(motor_axis, an, u8, MOTORS, 1, 1, "Maps motor to axis") -VAR(step_angle, sa, f32, MOTORS, 1, 1, "In degrees per full step") -VAR(travel, tr, f32, MOTORS, 1, 1, "Travel in mm/rev") -VAR(microstep, mi, u16, MOTORS, 1, 1, "Microsteps per full step") -VAR(reverse, rv, u8, MOTORS, 1, 1, "Reverse motor polarity") VAR(power_mode, pm, u8, MOTORS, 1, 1, "Motor power mode") VAR(drive_current, dc, f32, MOTORS, 1, 1, "Max motor drive current") VAR(idle_current, ic, f32, MOTORS, 1, 1, "Motor idle current") + +VAR(reverse, rv, u8, MOTORS, 1, 1, "Reverse motor polarity") +VAR(microstep, mi, u16, MOTORS, 1, 1, "Microsteps per full step") +VAR(velocity_max, vm, f32, MOTORS, 1, 1, "Maxium vel in mm/min") +VAR(accel_max, am, f32, MOTORS, 1, 1, "Maxium accel in mm/min^2") +VAR(jerk_max, jm, f32, MOTORS, 1, 1, "Maxium jerk in mm/min^3") +VAR(step_angle, sa, f32, MOTORS, 1, 1, "In degrees per full step") +VAR(travel, tr, f32, MOTORS, 1, 1, "Travel in mm/rev") + +VAR(min_soft_limit, tn, f32, MOTORS, 1, 1, "Min soft limit") +VAR(max_soft_limit, tm, f32, MOTORS, 1, 1, "Max soft limit") +VAR(homed, h, bool, MOTORS, 1, 1, "Motor homed status") + VAR(active_current, ac, f32, MOTORS, 0, 1, "Motor current now") VAR(driver_flags, df, u16, MOTORS, 0, 1, "Motor driver flags") VAR(status_strings, ds, flags, MOTORS, 0, 1, "Motor driver status") @@ -48,10 +58,6 @@ VAR(driver_stalled, sl, bool, MOTORS, 0, 1, "Motor driver status") VAR(encoder, en, s32, MOTORS, 0, 0, "Motor encoder") VAR(error, ee, s32, MOTORS, 0, 0, "Motor position error") -VAR(velocity_max, vm, f32, MOTORS, 1, 1, "Maxium vel in mm/min") -VAR(accel_max, am, f32, MOTORS, 1, 1, "Maxium accel in mm/min^2") -VAR(jerk_max, jm, f32, MOTORS, 1, 1, "Maxium jerk in mm/min^3") - VAR(motor_fault, fa, bool, 0, 0, 1, "Motor fault status") // Switches @@ -72,6 +78,9 @@ VAR(output_active, oa, bool, OUTS, 1, 1, "Output pin active") VAR(output_state, os, u8, OUTS, 0, 1, "Output pin state") VAR(output_mode, om, u8, OUTS, 1, 1, "Output pin mode") +// Analog +VAR(analog_input, ai, f32, ANALOG, 0, 0, "Analog input pins") + // Spindle VAR(spindle_type, st, u8, 0, 1, 1, "PWM=0 or HUANYANG=1") VAR(spin_reversed, sr, bool, 0, 1, 1, "Reverse spin") diff --git a/src/jade/index.jade b/src/jade/index.jade index a23d5ed..793936c 100644 --- a/src/jade/index.jade +++ b/src/jade/index.jade @@ -76,12 +76,15 @@ html(lang="en") li.pure-menu-heading a.pure-menu-link(href="#io") I/O - li.pure-menu-heading(style="display:none") + li.pure-menu-heading a.pure-menu-link(href="#gcode") Gcode li.pure-menu-heading a.pure-menu-link(href="#admin") Admin + li.pure-menu-heading + a.pure-menu-link(href="#cheat-sheet") Cheat Sheet + li.pure-menu-heading a.pure-menu-link(href="#help") Help @@ -134,7 +137,8 @@ html(lang="en") p.pure-control-group label(for="pass") Password - input(name="pass", v-model="password", type="password") + input(name="pass", v-model="password", type="password", + @keyup.enter="upgrade_confirmed") div(slot="footer") button.pure-button(@click="confirmUpgrade=false") Cancel @@ -148,7 +152,8 @@ html(lang="en") p.pure-control-group label(for="pass") Password - input(name="pass", v-model="password", type="password") + input(name="pass", v-model="password", type="password", + @keyup.enter="upload_confirmed") div(slot="footer") button.pure-button(@click="confirmUpload=false") Cancel @@ -158,16 +163,27 @@ html(lang="en") message(:show.sync="firmwareUpgrading") h3(slot="header") Firmware upgrading p(slot="body") Please wait... + div(slot="footer") message(:show.sync="showMessages") h3(slot="header") GCode message div(slot="body") ul - li(v-for="msg in messages") {{msg}} + li(v-for="msg in messages", track-by="$index") {{msg}} div(slot="footer") - button.pure-button.button-success(@click="close_messages") OK + button.pure-button.button-success(v-if="state.xx != 'HOLDING'", + @click="close_messages('ok')") OK + + div(v-if="state.xx == 'HOLDING'") + button.pure-button(@click="close_messages('stop')") + | Stop + .fa.fa-stop + + button.pure-button(@click="close_messages('continue')") + | Continue + .fa.fa-play #templates include ../../build/templates.jade diff --git a/src/jade/templates/help-view.jade b/src/jade/templates/help-view.jade index 76a1092..1c0187b 100644 --- a/src/jade/templates/help-view.jade +++ b/src/jade/templates/help-view.jade @@ -40,3 +40,22 @@ script#help-view-template(type="text/x-template") | forum.buildbotics.com. Register on the site and post a message. | We'll be happy to help. + + h2 CAD/CAM Software + p + | CAM software can be used to create GCode + | automatically from + | CAD models. Here are a few CAD/CAM resources: + ul + li: a(href="http://camotics.org/", target="_blank") + | CAMotics - Open-Source CNC Simulator + li: a(href="http://librecad.org/", target="_blank") + | LibreCAD - Open-Source 2D CAD + li: a(href="https://www.freecadweb.org/", target="_blank") + | FreeCAD - Open-Source 3D CAD + li: a(href="http://www.openscad.org/", target="_blank") + | OpenSCAD - Open-Source 3D CAD for programmers + li: a(href="http://wiki.linuxcnc.org/cgi-bin/wiki.pl?Cam", + target="_blank") LinuxCNC CAM resources diff --git a/src/jade/templates/indicators.jade b/src/jade/templates/indicators.jade index cdab3a9..b3d1163 100644 --- a/src/jade/templates/indicators.jade +++ b/src/jade/templates/indicators.jade @@ -160,6 +160,13 @@ script#indicators-template(type="text/x-template") td {{state['load2'] | fixed 2}} A th Load 2 + tr + td {{state['1ai'] | percent 0}} A + th Analog 1 + th.separator + td {{state['2ai'] | percent 0}} A + th Analog 2 + table.rs485 tr th.header(colspan=5) RS485 Spindle diff --git a/src/js/admin-view.js b/src/js/admin-view.js index d8fc91b..0be0e04 100644 --- a/src/js/admin-view.js +++ b/src/js/admin-view.js @@ -57,10 +57,6 @@ module.exports = { events: { - connected: function () { - if (this.firmwareUpgrading) location.reload(true); - }, - latest_version: function (version) {this.latest = version} }, diff --git a/src/js/app.js b/src/js/app.js index ef8ef55..f0404da 100644 --- a/src/js/app.js +++ b/src/js/app.js @@ -46,6 +46,35 @@ function compare_versions(a, b) { } +function is_object(o) {return o !== null && typeof o == 'object'} + + +function update_object(dst, src, remove) { + var props, index, key, value; + + if (remove) { + props = Object.getOwnPropertyNames(dst); + + for (index in props) { + key = props[index]; + if (!src.hasOwnProperty(key)) + Vue.delete(dst, key); + } + } + + props = Object.getOwnPropertyNames(src); + for (index in props) { + key = props[index]; + value = src[key]; + + if (is_object(value) && dst.hasOwnProperty(key) && is_object(dst[key])) + update_object(dst[key], value, remove); + + else Vue.set(dst, key, value); + } +} + + module.exports = new Vue({ el: 'body', @@ -57,13 +86,14 @@ module.exports = new Vue({ index: -1, modified: false, template: {motors: {}, axes: {}}, - config: {motors: [{}]}, + config: {motors: [{}, {}, {}, {}], admin: {}, version: ''}, state: {}, messages: [], errorTimeout: 30, errorTimeoutStart: 0, errorShow: false, errorMessage: '', + reloadOnConnect: false, confirmUpgrade: false, confirmUpload: false, firmwareUpgrading: false, @@ -85,7 +115,11 @@ module.exports = new Vue({ 'io-view': require('./io-view'), 'gcode-view': require('./gcode-view'), 'admin-view': require('./admin-view'), - 'help-view': {template: '#help-view-template'} + 'help-view': {template: '#help-view-template'}, + 'cheat-sheet-view': { + template: '#cheat-sheet-view-template', + data: function () {return {showUnimplemented: false}} + } }, @@ -101,7 +135,13 @@ module.exports = new Vue({ }, - connected: function () {this.update()}, + connected: function () { + if (this.reloadOnConnect) location.reload(true); + else this.update(); + }, + + + disconnected: function () {this.reloadOnConnect = true}, update: function () {this.update()}, @@ -216,8 +256,8 @@ module.exports = new Vue({ .success(function (data, status, xhr) { this.template = data; - api.get('config/load').done(function (data) { - this.config = data; + api.get('config/load').done(function (config) { + update_object(this.config, config, true); this.parse_hash(); if (!this.checkedUpgrade) { @@ -236,15 +276,20 @@ module.exports = new Vue({ this.sock = new Sock('//' + window.location.host + '/sockjs'); this.sock.onmessage = function (e) { - var msg = e.data; - - if (typeof msg == 'object') { - for (var key in msg) { - if (key == 'log') this.$broadcast('log', msg.log); - else if (key == 'message') this.add_message(msg.message); - else Vue.set(this.state, key, msg[key]); - } + if (typeof e.data != 'object') return; + + if ('log' in e.data) { + this.$broadcast('log', e.data.log); + delete e.data.log; } + + if ('message' in e.data) { + this.add_message(e.data.message); + delete e.data.message; + } + + update_object(this.state, e.data, false); + }.bind(this) this.sock.onopen = function (e) { @@ -255,6 +300,7 @@ module.exports = new Vue({ this.sock.onclose = function (e) { this.status = 'disconnected'; + this.$emit(this.status); this.$broadcast(this.status); }.bind(this) }, @@ -291,9 +337,12 @@ module.exports = new Vue({ }, - close_messages: function () { + close_messages: function (action) { this.showMessages = false; this.messages.splice(0, this.messages.length); + + if (action == 'stop') api.put('stop'); + if (action == 'continue') api.put('unpause'); } } }) diff --git a/src/py/bbctrl/Cmd.py b/src/py/bbctrl/Cmd.py index 7bcf41d..e7b167e 100644 --- a/src/py/bbctrl/Cmd.py +++ b/src/py/bbctrl/Cmd.py @@ -40,6 +40,7 @@ SET_SYNC = '#' SEEK = 's' SET_AXIS = 'a' LINE = 'l' +INPUT = 'I' DWELL = 'd' PAUSE = 'P' STOP = 'S' @@ -101,13 +102,36 @@ def line(target, exitVel, maxAccel, maxJerk, times): def speed(speed): return '#s=:' + encode_float(speed) +def input(port, mode, timeout): + type = 'd' + index = 0 + m = 0 + + if port == 'digital-in-0': digital, index = 'd', 0 + if port == 'digital-in-1': digital, index = 'd', 1 + if port == 'digital-in-2': digital, index = 'd', 2 + if port == 'digital-in-3': digital, index = 'd', 3 + if port == 'analog-in-0': digital, index = 'a', 0 + if port == 'analog-in-1': digital, index = 'a', 1 + if port == 'analog-in-2': digital, index = 'a', 2 + if port == 'analog-in-3': digital, index = 'a', 3 + + if mode == 'immediate': m = 0 + if mode == 'rise': m = 1 + if mode == 'fall': m = 2 + if mode == 'high': m = 3 + if mode == 'low': m = 4 + + return '%s%s%d%d%s' % (INPUT, type, index, m, encode_float(timeout)) + + def output(port, value): if port == 'mist': return '#1oa=' + ('1' if value else '0') if port == 'flood': return '#2oa=' + ('1' if value else '0') raise Exception('Unsupported output "%s"' % port) -def dwell(seconds): return 'd' + encode_float(seconds) +def dwell(seconds): return DWELL + encode_float(seconds) def pause(type): @@ -116,7 +140,7 @@ def pause(type): elif type == 'pallet-change': type = 2 else: raise Exception('Unknown pause type "%s"' % type) - return 'P%d' % type + return '%s%d' % (PAUSE, type) def jog(axes): return 'j' + encode_axes(axes) diff --git a/src/py/bbctrl/Comm.py b/src/py/bbctrl/Comm.py index 0281342..4d98f3a 100644 --- a/src/py/bbctrl/Comm.py +++ b/src/py/bbctrl/Comm.py @@ -140,7 +140,7 @@ class Comm(): def _update_vars(self, msg): try: - self.ctrl.state.machine_cmds_and_vars(msg) + self.ctrl.state.machine_vars(msg['variables']) self.queue_command(Cmd.DUMP) # Refresh all vars # Set axis positions diff --git a/src/py/bbctrl/Config.py b/src/py/bbctrl/Config.py index 5362774..d6c972e 100644 --- a/src/py/bbctrl/Config.py +++ b/src/py/bbctrl/Config.py @@ -48,6 +48,7 @@ default_config = { class Config(object): def __init__(self, ctrl): self.ctrl = ctrl + self.config_vars = {} try: self.version = pkg_resources.require('bbctrl')[0].version @@ -66,6 +67,14 @@ class Config(object): except Exception as e: log.exception(e) + def get(self, name, default = None): + return self.config_vars.get(name, default) + + + def get_index(self, name, index, default = None): + return self.config_vars.get(name, {}).get(str(index), None) + + def load_path(self, path): with open(path, 'r') as f: return json.load(f) @@ -121,7 +130,13 @@ class Config(object): def reset(self): os.unlink('config.json') - def encode_cmd(self, index, value, spec): + def _encode_cmd(self, name, index, value, spec): + if str(index): + if not name in self.config_vars: self.config_vars[name] = {} + self.config_vars[name][str(index)] = value + + else: self.config_vars[name] = value + if not 'code' in spec: return if spec['type'] == 'enum': @@ -135,29 +150,29 @@ class Config(object): self.ctrl.state.config(str(index) + spec['code'], value) - def encode_category(self, index, config, category, with_defaults): + def _encode_category(self, index, config, category, with_defaults): for key, spec in category.items(): if key in config: value = config[key] elif with_defaults: value = spec['default'] else: continue - self.encode_cmd(index, value, spec) + self._encode_cmd(key, index, value, spec) - def encode(self, index, config, tmpl, with_defaults): + def _encode(self, index, config, tmpl, with_defaults): for category in tmpl.values(): - self.encode_category(index, config, category, with_defaults) + self._encode_category(index, config, category, with_defaults) def update(self, config, with_defaults = False): for name, tmpl in self.template.items(): if name == 'motors': for index in range(len(config['motors'])): - self.encode(index, config['motors'][index], tmpl, - with_defaults) + self._encode(index, config['motors'][index], tmpl, + with_defaults) - else: self.encode_category('', config.get(name, {}), tmpl, - with_defaults) + else: self._encode_category('', config.get(name, {}), tmpl, + with_defaults) def reload(self): self.update(self.load(), True) diff --git a/src/py/bbctrl/I2C.py b/src/py/bbctrl/I2C.py index d485d09..02d76f0 100644 --- a/src/py/bbctrl/I2C.py +++ b/src/py/bbctrl/I2C.py @@ -48,7 +48,7 @@ class I2C(object): except OSError as e: self.i2c_bus = None - log.warning('Failed to open device: %s', e) + log.info('Failed to open device: %s', e) raise e @@ -59,7 +59,7 @@ class I2C(object): return self.i2c_bus.read_word_data(addr, 0) except IOError as e: - log.warning('I2C read word failed: %s' % e) + log.info('I2C read word failed: %s' % e) self.i2c_bus.close() self.i2c_bus = None raise e @@ -78,7 +78,7 @@ class I2C(object): else: self.i2c_bus.write_byte(addr, cmd) except IOError as e: - log.warning('I2C write failed: %s' % e) + log.info('I2C write failed: %s' % e) self.i2c_bus.close() self.i2c_bus = None raise e diff --git a/src/py/bbctrl/MainLCDPage.py b/src/py/bbctrl/MainLCDPage.py index 05ac51a..55fb1ca 100644 --- a/src/py/bbctrl/MainLCDPage.py +++ b/src/py/bbctrl/MainLCDPage.py @@ -39,27 +39,30 @@ class MainLCDPage(bbctrl.LCDPage): def update(self, update): + state = self.ctrl.state + # Must be after machine vars have loaded if self.install and hasattr(self, 'id'): self.install = False self.ctrl.lcd.set_current_page(self.id) - if 'xx' in update: - self.text('%-9s' % self.ctrl.state.get('xx'), 0, 0) + self.text('%-9s' % state.get('xx', ''), 0, 0) - # Show enabled axes + # Show enabled axes row = 0 for axis in 'xyzabc': - motor = self.ctrl.state.find_motor(axis) - if motor is not None: - if (axis + 'p') in update: - self.text('% 10.3f%s' % (update[axis + 'p'], axis.upper()), - 9, row) - + if state.is_axis_enabled(axis): + position = state.get(axis + 'p', 0) + self.text('% 10.3f%s' % (position, axis.upper()), 9, row) row += 1 + while row < 4: + self.text(' ' * 11, 9, row) + row += 1 + # Show tool, units, feed and speed - if 'tool' in update: self.text('%2uT' % update['tool'], 6, 1) - if 'units' in update: self.text('%-6s' % update['units'], 0, 1) - if 'feed' in update: self.text('%8uF' % update['feed'], 0, 2) - if 'speed' in update: self.text('%8dS' % update['speed'], 0, 3) + units = 'INCH' if state.get('imperial', False) else 'MM' + self.text('%2uT' % state.get('tool', 0), 6, 1) + self.text('%-6s' % units, 0, 1) + self.text('%8uF' % state.get('feed', 0), 0, 2) + self.text('%8dS' % state.get('speed',0), 0, 3) diff --git a/src/py/bbctrl/Planner.py b/src/py/bbctrl/Planner.py index f91d11d..4e96ab4 100644 --- a/src/py/bbctrl/Planner.py +++ b/src/py/bbctrl/Planner.py @@ -26,6 +26,7 @@ ################################################################################ import json +import math import re import logging from collections import deque @@ -41,7 +42,7 @@ reLogLine = re.compile( class Planner(): def __init__(self, ctrl): self.ctrl = ctrl - self.lastID = -1 + self.lastID = 0 self.setq = deque() ctrl.state.add_listener(self._update) @@ -83,14 +84,41 @@ class Planner(): return v - def _get_config(self): + def _get_soft_limit(self, var, default): + limit = self._get_config_vector(var, 1) + + for axis in 'xyzabc': + if not axis in limit or not self.ctrl.state.is_axis_homed(axis): + limit[axis] = default + + return limit + + + def _get_config(self, mdi): config = { + "min-soft-limit": self._get_soft_limit('tn', -math.inf), + "max-soft-limit": self._get_soft_limit('tm', math.inf), "max-vel": self._get_config_vector('vm', 1000), "max-accel": self._get_config_vector('am', 1000000), "max-jerk": self._get_config_vector('jm', 1000000), # TODO junction deviation & accel } + if not mdi: + program_start = self.ctrl.config.get('program-start') + if program_start: config['program-start'] = program_start + + + overrides = {} + + tool_change = self.ctrl.config.get('tool-change') + if tool_change: overrides['M6'] = tool_change + + program_end = self.ctrl.config.get('program-end') + if program_end: overrides['M2'] = program_end + + if overrides: config['overrides'] = overrides + log.info('Config:' + json.dumps(config)) return config @@ -101,7 +129,7 @@ class Planner(): id = update['id'] self.planner.set_active(id) - # Syncronize planner variables with execution id + # Synchronize planner variables with execution id self._release_set_cmds(id) @@ -169,14 +197,23 @@ class Planner(): if name == 'speed': return Cmd.speed(value) - if (name[0:1] == '_' and name[1:2] in 'xyzabc' and - name[2:] == '_home'): return Cmd.set_axis(name[1], value) - if len(name) and name[0] == '_': self._queue_set_cmd(block['id'], name[1:], value) + if name[0:1] == '_' and name[1:2] in 'xyzabc': + if name[2:] == '_home': return Cmd.set_axis(name[1], value) + + if name[2:] == '_homed': + motor = self.ctrl.state.find_motor(name[1]) + if motor is not None: return Cmd.set('%dh' % motor, value) + return + if type == 'input': + # TODO handle timeout + self.planner.synchronize(0) # TODO Fix this + return Cmd.input(block['port'], block['mode'], block['timeout']) + if type == 'output': return Cmd.output(block['port'], int(float(block['value']))) @@ -185,7 +222,7 @@ class Planner(): if type == 'seek': return Cmd.seek(block['switch'], block['active'], block['error']) - raise Exception('Unknown planner type "%s"' % type) + raise Exception('Unknown planner command "%s"' % type) def _encode(self, block): @@ -202,7 +239,7 @@ class Planner(): def stop(self): self.planner.stop() - self.lastID = -1 + self.lastID = 0 self.setq.clear() @@ -217,16 +254,18 @@ class Planner(): log.info('Planner restart: %d %s' % (id, json.dumps(position))) self.planner.restart(id, position) + if self.planner.is_synchronizing(): self.planner.synchronize(1) + # TODO if these calls fail we get stuck in a loop def mdi(self, cmd): log.info('MDI:' + cmd) - self.planner.load_string(cmd, self._get_config()) + self.planner.load_string(cmd, self._get_config(True)) def load(self, path): log.info('GCode:' + path) - self.planner.load(path, self._get_config()) + self.planner.load(path, self._get_config(False)) def has_move(self): return self.planner.has_more() diff --git a/src/py/bbctrl/Pwr.py b/src/py/bbctrl/Pwr.py index 337c85f..3a6e132 100644 --- a/src/py/bbctrl/Pwr.py +++ b/src/py/bbctrl/Pwr.py @@ -128,7 +128,7 @@ class Pwr(): if i == FLAGS_REG: self.check_faults() except Exception as e: - log.warning('Pwr communication failed: %s' % e) + log.info('Pwr communication failed: %s' % e) self.ctrl.ioloop.call_later(1, self._update) return diff --git a/src/py/bbctrl/State.py b/src/py/bbctrl/State.py index 516d6d4..f84c0d2 100644 --- a/src/py/bbctrl/State.py +++ b/src/py/bbctrl/State.py @@ -40,9 +40,7 @@ class State(object): self.changes = {} self.listeners = [] self.timeout = None - self.machine_vars = {} self.machine_var_set = set() - self.machine_cmds = {} # Defaults self.vars = { @@ -130,6 +128,7 @@ class State(object): def config(self, code, value): + # Set machine variables via mach, others directly if code in self.machine_var_set: self.ctrl.mach.set(code, value) else: self.set(code, value) @@ -142,19 +141,17 @@ class State(object): def remove_listener(self, listener): self.listeners.remove(listener) - def machine_cmds_and_vars(self, data): - self.machine_cmds = data['commands'] - self.machine_vars = data['variables'] - - # Record all machine vars, indexed or not + def machine_vars(self, vars): + # Record all machine vars, indexed or otherwise self.machine_var_set = set() - for code, spec in self.machine_vars.items(): + for code, spec in vars.items(): if 'index' in spec: for index in spec['index']: self.machine_var_set.add(index + code) else: self.machine_var_set.add(code) - self.ctrl.config.reload() # Indirectly configures AVR + # Indirectly configure mach via calls to config() + self.ctrl.config.reload() def find_motor(self, axis): @@ -170,7 +167,17 @@ class State(object): def is_axis_enabled(self, axis): motor = self.find_motor(axis) - return False if motor is None else self.motor_enabled(motor) + return motor is not None and self.motor_enabled(motor) + + + def get_enabled_axes(self): + axes = [] + + for axis in 'xyzabc': + if self.is_axis_enabled(axis): + axes.append(axis) + + return axes def axis_homing_mode(self, axis): diff --git a/src/py/bbctrl/__init__.py b/src/py/bbctrl/__init__.py index bd0fd5c..e060631 100644 --- a/src/py/bbctrl/__init__.py +++ b/src/py/bbctrl/__init__.py @@ -102,7 +102,7 @@ def run(): level = logging.DEBUG if args.verbose else logging.INFO root.setLevel(logging.NOTSET) f = logging.Formatter('{levelname[0]}:{name}:{message}', style = '{') - h = logging.StreamHandler(level) + h = logging.StreamHandler() h.setLevel(level) h.setFormatter(f) root.addHandler(h) diff --git a/src/resources/config-template.json b/src/resources/config-template.json index bb35333..b05111b 100644 --- a/src/resources/config-template.json +++ b/src/resources/config-template.json @@ -272,17 +272,17 @@ }, "gcode": { - "preamble": { + "program-start": { "type": "text", - "default": "G21 (Operate in millimeters)\nG90 (Absolute distance mode)\nG17 (Select XY plane)\nG40 (Radius compensation off)\nG49 (Tool length compensation off)\nG61 (Exact path mode)\n" + "default": "(Runs at program start)\nG21 (Metric units)\nG90 (Absolute distance mode)\nG17 (Select XY plane)\n" }, "tool-change": { "type": "text", - "default": "M6 (Tool change)\n" + "default": "(Runs on M6, tool change)\nM0 M6 (MSG, Change tool)" }, - "epilogue": { + "program-end": { "type": "text", - "default": "M2 (End program)\n" + "default": "(Runs on M2, program end)\nM2" } }, diff --git a/src/stylus/style.styl b/src/stylus/style.styl index ff31f1d..efc8456 100644 --- a/src/stylus/style.styl +++ b/src/stylus/style.styl @@ -602,6 +602,34 @@ label.file-upload margin-bottom 1em clear both +.cheat-sheet + table + border-collapse collapse + margin-bottom 0.5em + + * + text-align left + + th, td + border 1px solid #eee + padding 4px + + tr.header-row + background #eee + + tr:nth-child(even) + background #f7f7f7 + + tr.unimplemented + background #ffff9b + + tr.spacer-row + background transparent + + th, td + border none + height 1em + @media only screen and (max-width 48em) .header