``_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 differencing is used to map
+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.
non-zero segments have been executed, the move is complete.
## Pulse generation
-Calls to ``st_prep_line()`` prepare short (~5ms) move fragements for pluse
+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 ISR, after ending the previous move, calls
+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 ISR signals a lower level interrupt to
-call ``mp_exec_move()`` and load the next move fragment.
+After (re)starting the motor clocks the interrupt signals a lower level
+interrupt to call ``mp_exec_move()`` and load the next move fragment.
float velocity_max; // max velocity in mm/min or deg/min
float travel_max; // max work envelope for soft limits
float travel_min; // min work envelope for soft limits
- float jerk_max; // max jerk (Jm) in mm/min^3 divided by 1 million
- float recip_jerk; // reciprocal of current jerk value - with million
+ float jerk_max; // max jerk (Jm) in km/min^3
+ float recip_jerk; // reciprocal of current jerk in min^3/mm
float radius; // radius in mm for rotary axes
float search_velocity; // homing search velocity
float latch_velocity; // homing latch velocity
bool axis_is_enabled(int axis) {
int motor = axis_get_motor(axis);
- return motor != -1 && motor_is_enabled(motor);
+ return motor != -1 && motor_is_enabled(motor) &&
+ !fp_ZERO(axis_get_velocity_max(axis));
}
#include "plan/arc.h"
#include "plan/state.h"
#include "config.h"
+#include "pgmspace.h"
-#include <avr/pgmspace.h>
+#ifdef __AVR__
#include <avr/wdt.h>
+#endif
#include <stdio.h>
#include <string.h>
// Command functions
void static print_command_help(int i) {
- static const char fmt[] PROGMEM = " $%-12S %S\n";
+ static const char fmt[] PROGMEM = " $%-12"PRPSTR" %"PRPSTR"\n";
const char *name = pgm_read_word(&commands[i].name);
const char *help = pgm_read_word(&commands[i].help);
const char *name = pgm_read_word(&commands[i].name);
if (!name) break;
print_command_help(i);
+#ifdef __AVR__
wdt_reset();
+#endif
}
puts_P(PSTR("\nVariables:"));
#include "pins.h"
+#ifdef __AVR__
#include <avr/interrupt.h>
+#endif
// Pins
#include <ctype.h>
#include <stdlib.h>
#include <math.h>
+#include <stdio.h>
parser_t parser = {{0}};
// For now this is unconditional and will always delete
if (block_delete_flag) return STAT_NOOP;
+#ifdef DEBUG
+ printf("GCODE: %s\n", block);
+#endif
+
// queue a "(MSG" response
if (*msg) mach_message(msg); // queue the message
#pragma once
#include "config.h"
-
-#include <avr/pgmspace.h>
+#include "pgmspace.h"
#include <stdint.h>
#include <stdbool.h>
typedef enum {
- INVERSE_TIME_MODE, // G93
UNITS_PER_MINUTE_MODE, // G94
+ INVERSE_TIME_MODE, // G93
UNITS_PER_REVOLUTION_MODE, // G95 (unimplemented)
} feed_mode_t;
#include "usart.h"
#include "huanyang.h"
#include "config.h"
+#include "pgmspace.h"
#include <avr/interrupt.h>
-#include <avr/pgmspace.h>
#include <avr/eeprom.h>
#include <avr/wdt.h>
#include "config.h"
#include "rtc.h"
#include "report.h"
+#include "pgmspace.h"
#include <avr/io.h>
#include <avr/interrupt.h>
-#include <avr/pgmspace.h>
#include <util/crc16.h>
#include <stdio.h>
#include "homing.h"
#include "home.h"
#include "i2c.h"
+#include "pgmspace.h"
#include "plan/planner.h"
#include "plan/arc.h"
#include "plan/state.h"
-#include <avr/pgmspace.h>
#include <avr/wdt.h>
#include <stdio.h>
#include "gcode_state.h"
#include "axis.h"
#include "util.h"
+#include "pgmspace.h"
#include "plan/runtime.h"
#include "plan/calibrate.h"
-#include <avr/interrupt.h>
-#include <avr/pgmspace.h>
-
#include <string.h>
#include <math.h>
#include <stdio.h>
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 <http://www.gnu.org/licenses/>.
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#ifdef __AVR__
+
+#include <avr/pgmspace.h>
+#define PRPSTR "S"
+
+#else // __AVR__
+
+#define PRPSTR "s"
+#define PROGMEM
+#define PGM_P char *
+#define PSTR(X) X
+#define vfprintf_P vfprintf
+#define printf_P printf
+#define puts_P puts
+#define sprintf_P sprintf
+#define strcmp_P strcmp
+#define pgm_read_ptr(x) *(x)
+#define pgm_read_word(x) *(x)
+#define pgm_read_byte(x) *(x)
+
+#endif // __AVR__
#pragma once
-#include <avr/io.h>
-
-
enum {PORT_A = 1, PORT_B, PORT_C, PORT_D, PORT_E, PORT_F};
-extern PORT_t *pin_ports[];
-
#define PORT(PIN) pin_ports[(PIN >> 3) - 1]
#define BM(PIN) (1 << (PIN & 7))
+#ifdef __AVR__
+#include <avr/io.h>
+
+extern PORT_t *pin_ports[];
+
#define DIRSET_PIN(PIN) PORT(PIN)->DIRSET = BM(PIN)
#define DIRCLR_PIN(PIN) PORT(PIN)->DIRCLR = BM(PIN)
#define OUTCLR_PIN(PIN) PORT(PIN)->OUTCLR = BM(PIN)
#define OUT_PIN(PIN) (!!(PORT(PIN)->OUT & BM(PIN)))
#define IN_PIN(PIN) (!!(PORT(PIN)->IN & BM(PIN)))
#define PINCTRL_PIN(PIN) ((&PORT(PIN)->PIN0CTRL)[PIN & 7])
+
+#endif // __AVR__
* invalidating its contents
*/
void mp_queue_push(buffer_cb_t cb, uint32_t line) {
+ mp_buffer_validate(mb.tail);
mp_state_running();
mb.tail->ts = rtc_get_time();
_clear_buffer(mb.head);
_pop();
}
+
+
+#ifdef DEBUG
+void mp_queue_dump() {
+ mp_buffer_t *bf = mp_queue_get_head();
+ if (!bf) return;
+ mp_buffer_t *bp = bf;
+
+ do {
+ if (bp != bf) putchar(',');
+ mp_buffer_print(bp);
+ bp = mp_buffer_next(bp);
+ } while (bp != bf && bp->state != BUFFER_OFF);
+
+ if (bp != bf) mp_buffer_print(bp);
+
+ putchar('\n');
+}
+
+
+void mp_buffer_print(const mp_buffer_t *bf) {
+ printf("{\n"
+ " ts: %d,\n"
+ " line: %d,\n"
+ " state: %d,\n"
+ " replannable: %s,\n"
+ " hold: %s,\n"
+ " value: %0.2f,\n"
+ " target: [%0.2f, %0.2f, %0.2f, %0.2f],\n"
+ " unit: [%0.2f, %0.2f, %0.2f, %0.2f],\n"
+ " length: %0.2f,\n"
+ " head_length: %0.2f,\n"
+ " body_length: %0.2f,\n"
+ " tail_length: %0.2f,\n"
+ " entry_velocity: %0.2f,\n"
+ " cruise_velocity: %0.2f,\n"
+ " exit_velocity: %0.2f,\n"
+ " braking_velocity: %0.2f,\n"
+ " entry_vmax: %0.2f,\n"
+ " cruise_vmax: %0.2f,\n"
+ " exit_vmax: %0.2f,\n"
+ " delta_vmax: %0.2f,\n"
+ " jerk: %0.2f,\n"
+ " cbrt_jerk: %0.2f\n"
+ "}", bf->ts, bf->line, bf->state, bf->replannable ? "true" : "false",
+ bf->hold ? "true" : "false", bf->value, bf->target[0], bf->target[1],
+ bf->target[2], bf->target[3], bf->unit[0], bf->unit[1], bf->unit[2],
+ bf->unit[3], bf->length, bf->head_length, bf->body_length,
+ bf->tail_length, bf->entry_velocity, bf->cruise_velocity,
+ bf->exit_velocity, bf->braking_velocity, bf->entry_vmax,
+ bf->cruise_vmax, bf->exit_vmax, bf->delta_vmax, bf->jerk,
+ bf->cbrt_jerk);
+}
+#endif // DEBUG
+
+
+void mp_buffer_validate(const mp_buffer_t *bp) {
+ ASSERT(bp);
+
+ ASSERT(isfinite(bp->value));
+
+ ASSERT(isfinite(bp->target[0]) && isfinite(bp->target[1]) &&
+ isfinite(bp->target[2]) && isfinite(bp->target[3]));
+ ASSERT(isfinite(bp->unit[0]) && isfinite(bp->unit[1]) &&
+ isfinite(bp->unit[2]) && isfinite(bp->unit[3]));
+
+ ASSERT(isfinite(bp->length));
+ ASSERT(isfinite(bp->head_length));
+ ASSERT(isfinite(bp->body_length));
+ ASSERT(isfinite(bp->tail_length));
+
+ ASSERT(isfinite(bp->entry_velocity));
+ ASSERT(isfinite(bp->cruise_velocity));
+ ASSERT(isfinite(bp->exit_velocity));
+ ASSERT(isfinite(bp->braking_velocity));
+
+ ASSERT(isfinite(bp->entry_vmax));
+ ASSERT(isfinite(bp->cruise_vmax));
+ ASSERT(isfinite(bp->exit_vmax));
+ ASSERT(isfinite(bp->delta_vmax));
+
+ ASSERT(isfinite(bp->jerk));
+ ASSERT(isfinite(bp->cbrt_jerk));
+}
float value; // used in dwell and other callbacks
- float target[AXES]; // XYZABC where the move should go
+ float target[AXES]; // XYZABC where the move should go in mm
float unit[AXES]; // unit vector for axis scaling & planning
float length; // total length of line or helix in mm
mp_buffer_t *mp_queue_get_head();
void mp_queue_pop();
+void mp_queue_dump();
+
+void mp_buffer_print(const mp_buffer_t *bp);
+void mp_buffer_validate(const mp_buffer_t *bp);
static inline mp_buffer_t *mp_buffer_prev(mp_buffer_t *bp) {return bp->prev;}
static inline mp_buffer_t *mp_buffer_next(mp_buffer_t *bp) {return bp->next;}
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
+#include <inttypes.h>
#define CAL_VELOCITIES 256
#include "status.h"
+#include <stdint.h>
+
+
stat_t mp_dwell(float seconds, int32_t line);
#include <stdio.h>
+typedef struct {
+ float a, b, c, d;
+ float delta;
+ float half_delta;
+ uint16_t count;
+} quintic_bezier_t;
+
+
typedef struct {
float unit[AXES]; // unit vector for axis scaling & planning
float final_target[AXES]; // final target, used to correct rounding errors
float segment_velocity; // computed velocity for segment
float segment_time; // actual time increment per segment
forward_dif_t fdif; // forward difference levels
+ quintic_bezier_t qb;
bool hold_planned; // true when a feedhold has been planned
move_section_t section; // current move section
bool section_new; // true if it's a new section
static mp_exec_t ex = {{0}};
+static float _quintic_bezier_next() {
+ float t = ex.qb.half_delta + ex.qb.count++ * ex.qb.delta;
+ float t2 = t * t;
+ float t3 = t2 * t;
+
+ return (ex.qb.a * t2 + ex.qb.b * t + ex.qb.c) * t3 + ex.qb.d;
+}
+
+
+static float _quintic_bezier_init(float Vi, float Vt, float segs) {
+ ex.qb.a = 6 * (Vt - Vi);
+ ex.qb.b = -15 * (Vt - Vi);
+ ex.qb.c = 10 * (Vt - Vi);
+ ex.qb.d = Vi;
+ ex.qb.delta = 1 / segs;
+ ex.qb.half_delta = ex.qb.delta / 2;
+ ex.qb.count = 0;
+
+ return _quintic_bezier_next();
+}
+
+
static stat_t _exec_aline_segment() {
float target[AXES];
/// Common code for head and tail sections
static stat_t _exec_aline_section(float length, float vin, float vout) {
if (ex.section_new) {
+ ASSERT(isfinite(length));
+
if (fp_ZERO(length)) return STAT_NOOP; // end the section
+ ASSERT(isfinite(vin) && isfinite(vout));
+ ASSERT(0 <= vin && 0 <= vout);
+ ASSERT(vin || vout);
+
// len / avg. velocity
float move_time = 2 * length / (vin + vout);
float segments = ceil(move_time / NOM_SEGMENT_TIME);
ex.segment_count = round(segments);
if (vin == vout) ex.segment_velocity = vin;
- else ex.segment_velocity =
- mp_init_forward_dif(ex.fdif, vin, vout, segments);
+ else {
+#if 0
+ ex.segment_velocity =
+ mp_init_forward_dif(ex.fdif, vin, vout, segments);
+#else
+ ex.segment_velocity = _quintic_bezier_init(vin, vout, segments);
+#endif
+ }
if (ex.segment_time < MIN_SEGMENT_TIME)
return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position
ex.section_new = false;
} else {
- if (vin != vout) ex.segment_velocity += mp_next_forward_dif(ex.fdif);
+ if (vin != vout) {
+#if 0
+ ex.segment_velocity += mp_next_forward_dif(ex.fdif);
+#else
+ ex.segment_velocity = _quintic_bezier_next();
+#endif
+ }
// Subsequent segments
if (_exec_aline_segment() == STAT_OK) return STAT_OK;
#include "forward_dif.h"
+#include "util.h"
+
#include <math.h>
/// Forward differencing math
///
/// We are using a quintic (fifth-degree) Bezier polynomial for the velocity
-/// curve. This gives us a "linear pop" velocity curve; with pop being the
-/// sixth derivative of position: velocity - 1st, acceleration - 2nd, jerk -
-/// 3rd, snap - 4th, crackle - 5th, pop - 6th
+/// curve. This gives us a constant "pop"; with pop being the sixth derivative
+/// of position:
+///
+/// 1st - velocity
+/// 2nd - acceleration
+/// 3rd - jerk
+/// 4th - snap
+/// 5th - crackle
+/// 6th - pop
///
/// The Bezier curve takes the form:
///
/// E = - 5 * P_0 + 5 * P_1
/// F = P_0
///
-/// Now, since we will (currently) *always* want the initial acceleration and
-/// jerk values to be 0, We set P_i = P_0 = P_1 = P_2 (initial velocity), and
-/// P_t = P_3 = P_4 = P_5 (target velocity), which, after simplification,
-/// resolves to:
+/// Since we want the initial acceleration and jerk values to be 0, We set
+///
+/// P_i = P_0 = P_1 = P_2 (initial velocity)
+/// P_t = P_3 = P_4 = P_5 (target velocity)
+///
+/// which, after simplification, resolves to:
///
/// A = - 6 * P_i + 6 * P_t
/// B = 15 * P_i - 15 * P_t
/// E = 0
/// F = P_i
///
-/// Given an interval count of I to get from P_i to P_t, we get the parametric
-/// "step" size of h = 1/I. We need to calculate the initial value of forward
-/// differences (F_0 - F_5) such that the inital velocity V = P_i, then we
-/// iterate over the following I times:
+/// Given an interval count of segments to get from P_i to P_t, we get the
+/// parametric "step" size:
+///
+/// h = 1 / segs
+///
+/// We need to calculate the initial value of forward differences (F_0 - F_5)
+/// such that the inital velocity V = P_i, then we iterate over the following
+/// segs times:
///
/// V += F_5
/// F_5 += F_4
/// Normally, we could then assign t = 0, use the A-F values from above, and get
/// out initial F_* values. However, for the sake of "averaging" the velocity
/// of each segment, we actually want to have the initial V be be at t = h/2 and
-/// iterate I-1 times. So, the resulting F_* values are (steps not shown):
+/// iterate segs-1 times. So, the resulting F_* values are (steps not shown):
///
/// F_5 = 121Ah^5 / 16 + 5Bh^4 + 13Ch^3 / 4 + 2Dh^2 + Eh
/// F_4 = 165Ah^5 / 2 + 29Bh^4 + 9Ch^3 + 2Dh^2
/// F_2 = ( - 360 * s + 1800.0 )(Vt - Vi) * h^5
/// F_1 = ( 720.0 )(Vt - Vi) * h^5
///
-float mp_init_forward_dif(forward_dif_t fdif, float Vi, float Vt, float s) {
- const float h = 1 / s;
- const float s2 = square(s);
+float mp_init_forward_dif(forward_dif_t fdif, float Vi, float Vt, float segs) {
+ const float h = 1 / segs;
+ const float segs2 = square(segs);
const float Vdxh5 = (Vt - Vi) * h * h * h * h * h;
- fdif[4] = (32.5 * s2 - 75.0 * s + 45.375) * Vdxh5;
- fdif[3] = (90.0 * s2 - 435.0 * s + 495.0 ) * Vdxh5;
- fdif[2] = (60.0 * s2 - 720.0 * s + 1530.0 ) * Vdxh5;
- fdif[1] = ( - 360.0 * s + 1800.0 ) * Vdxh5;
- fdif[0] = ( 720.0 ) * Vdxh5;
+ fdif[4] = (32.5 * segs2 - 75.0 * segs + 45.375) * Vdxh5;
+ fdif[3] = (90.0 * segs2 - 435.0 * segs + 495.0 ) * Vdxh5;
+ fdif[2] = (60.0 * segs2 - 720.0 * segs + 1530.0 ) * Vdxh5;
+ fdif[1] = ( - 360.0 * segs + 1800.0 ) * Vdxh5;
+ fdif[0] = ( 720.0 ) * Vdxh5;
// Calculate the initial velocity by calculating:
//
// V(h / 2) =
//
- // ( -6Vi + 6Vt) / (2^5 * s^8) +
+ // ( -6Vi + 6Vt) / (2^5 * s^8) +
// ( 15Vi - 15Vt) / (2^4 * s^8) +
// (-10Vi + 10Vt) / (2^3 * s^8) + Vi =
//
#include "exec.h"
#include "buffer.h"
+#include <stdio.h>
+
/* Sonny's algorithm - simple
*
static void _calc_max_velocities(mp_buffer_t *bf, float move_time,
bool exact_stop) {
+ ASSERT(0 < move_time && isfinite(move_time));
+
float junction_velocity =
_get_junction_vmax(mp_buffer_prev(bf)->unit, bf->unit);
const float axis_square[], bool rapid,
bool inverse_time, float feed_rate,
float feed_override) {
+ ASSERT(0 < feed_override);
float max_time = 0;
// Compute times for feed motion
stat_t mp_aline(const float target[], bool rapid, bool inverse_time,
bool exact_stop, float feed_rate, float feed_override,
int32_t line) {
+ DEBUG_CALL("(%f, %f, %f, %f), %s, %s, %s, %f, %f, %d", target[0], target[1],
+ target[2], target[3], rapid ? "true" : "false",
+ inverse_time ? "true" : "false", exact_stop ? "true" : "false",
+ feed_rate, feed_override, line);
+
// Compute axis and move lengths
float axis_length[AXES];
float axis_square[AXES];
#include "status.h"
#include <stdbool.h>
+#include <stdint.h>
stat_t mp_aline(const float target[], bool rapid, bool inverse_time,
* tons of pathologically short Gcode blocks are being thrown at you.
*/
void mp_calculate_trapezoid(mp_buffer_t *bf) {
- // RULE #1 of mp_calculate_trapezoid(): Don't change bf->length
-
if (!bf->length) return;
// F case: Block is too short - run time < minimum segment time
if (naive_move_time <= NOM_SEGMENT_TIME) {
bf->entry_velocity = mp_buffer_prev(bf)->exit_velocity;
- if (fp_NOT_ZERO(bf->entry_velocity)) {
+ if (!fp_ZERO(bf->entry_velocity)) {
bf->cruise_velocity = bf->entry_velocity;
bf->exit_velocity = bf->entry_velocity;
mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf->jerk);
bf->tail_length =
mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf->jerk);
- if (bf->head_length < MIN_HEAD_LENGTH) { bf->head_length = 0;}
- if (bf->tail_length < MIN_TAIL_LENGTH) { bf->tail_length = 0;}
+ if (bf->head_length < MIN_HEAD_LENGTH) bf->head_length = 0;
+ if (bf->tail_length < MIN_TAIL_LENGTH) bf->tail_length = 0;
// Rate-limited HT and HT' cases
if (bf->length < (bf->head_length + bf->tail_length)) { // it's rate limited
// Symmetric rate-limited case (HT)
if (fabs(bf->entry_velocity - bf->exit_velocity) <
TRAPEZOID_VELOCITY_TOLERANCE) {
- bf->head_length = bf->length/2;
+ bf->head_length = bf->length / 2;
bf->tail_length = bf->head_length;
bf->cruise_velocity =
min(bf->cruise_vmax,
bf->tail_length = 0;
// Average the entry speed and computed best cruise-speed
- bf->cruise_velocity = (bf->entry_velocity + bf->cruise_velocity)/2;
+ bf->cruise_velocity = (bf->entry_velocity + bf->cruise_velocity) / 2;
bf->entry_velocity = bf->cruise_velocity;
bf->exit_velocity = bf->cruise_velocity;
}
bf->tail_length =
mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf->jerk);
- if (bf->head_length > bf->tail_length) {
+ if (bf->tail_length < bf->head_length) {
bf->head_length =
(bf->head_length / (bf->head_length + bf->tail_length)) * bf->length;
computed_velocity =
bf->body_length = bf->length - bf->head_length - bf->tail_length;
// If a non-zero body is < minimum length distribute it to the head and/or
- // tail. This will generate small (acceptable) velocity errors in runtime
+ // tail. This will generate small (acceptable) velocity errors in runtime
// execution but preserve correct distance, which is more important.
- if ((bf->body_length < MIN_BODY_LENGTH) && (fp_NOT_ZERO(bf->body_length))) {
- if (fp_NOT_ZERO(bf->head_length)) {
- if (fp_NOT_ZERO(bf->tail_length)) { // HBT reduces to HT
+ if (bf->body_length < MIN_BODY_LENGTH && !fp_ZERO(bf->body_length)) {
+ if (!fp_ZERO(bf->head_length)) {
+ if (!fp_ZERO(bf->tail_length)) { // HBT reduces to HT
bf->head_length += bf->body_length / 2;
bf->tail_length += bf->body_length / 2;
- } else bf->head_length += bf->body_length; // HB reduces to H
- } else bf->tail_length += bf->body_length; // BT reduces to T
+ } else bf->head_length += bf->body_length; // HB reduces to H
+ } else bf->tail_length += bf->body_length; // BT reduces to T
bf->body_length = 0;
// If the body is a standalone make the cruise velocity match the entry
- // velocity. This removes a potential velocity discontinuity at the expense
+ // velocity. This removes a potential velocity discontinuity at the expense
// of top speed
- } else if ((fp_ZERO(bf->head_length)) && (fp_ZERO(bf->tail_length)))
+ } else if (fp_ZERO(bf->head_length) && fp_ZERO(bf->tail_length))
bf->cruise_velocity = bf->entry_velocity;
}
int i = 0;
mp_buffer_t *bp = bf;
while (bp) {
- printf_P(PSTR("{\"msg\":\"%d,%d,0x%04x,"
+ printf_P(PSTR("{\"msg\":\"%d,%d,0x%04lx,"
"%0.2f,%0.2f,%0.2f,%0.2f,"
"%0.2f,%0.2f,%0.2f,%0.2f,"
"%0.2f,%0.2f,%0.2f\"}\n"),
- i++, bp->replannable, bp->cb,
+ i++, bp->replannable, (intptr_t)bp->cb,
bp->length, bp->head_length, bp->body_length, bp->tail_length,
bp->entry_velocity, bp->cruise_velocity, bp->exit_velocity,
bp->braking_velocity,
* PLANNER_VELOCITY_TOLERANCE necessitating the introduction of fabs()
*/
float mp_get_target_length(float Vi, float Vf, float jerk) {
+ ASSERT(0 <= Vi && 0 <= Vf);
+ ASSERT(isfinite(jerk));
return fp_EQ(Vi, Vf) ? 0 : fabs(Vi - Vf) * invsqrt(jerk / fabs(Vi - Vf));
}
* J'(x) = (2 * Vi * x - Vi^2 + 3 * x^2) / L^2
*/
float mp_get_target_velocity(float Vi, float L, const mp_buffer_t *bf) {
+ ASSERT(0 <= Vi && 0 <= L);
+ ASSERT(isfinite(bf->cbrt_jerk) && isfinite(bf->jerk));
+
+ if (!L) return Vi;
+
// 0 iterations (a reasonable estimate)
float x = pow(L, 0.66666666) * bf->cbrt_jerk + Vi; // First estimate
-#if (GET_VELOCITY_ITERATIONS > 0)
+#if GET_VELOCITY_ITERATIONS
const float L2 = L * L;
const float Vi2 = Vi * Vi;
/// Segment runner
stat_t mp_runtime_move_to_target(float target[], float time) {
+ ASSERT(isfinite(time));
+
// Convert target position to steps.
float steps[MOTORS];
mp_kinematics(target, steps);
#pragma once
-#include <avr/pgmspace.h>
+#include "pgmspace.h"
#include <stdbool.h>
#include "spindle.h"
#include "switch.h"
#include "util.h"
+#include "pgmspace.h"
#include "plan/planner.h"
#include "plan/runtime.h"
#include "plan/state.h"
-#include <avr/pgmspace.h>
-
#include <math.h>
#include <string.h>
#include <stdbool.h>
#include "usart.h"
#include "rtc.h"
#include "vars.h"
-
-#include <avr/pgmspace.h>
+#include "pgmspace.h"
#include <stdio.h>
#include <stdbool.h>
stat_t status_code; // allocate a variable for the RITORNO macro
+
#define STAT_MSG(NAME, TEXT) static const char stat_##NAME[] PROGMEM = TEXT;
#include "messages.def"
#undef STAT_MSG
+
static const char *const stat_msg[] PROGMEM = {
#define STAT_MSG(NAME, TEXT) stat_##NAME,
#include "messages.def"
va_list args;
// Type
- printf_P(PSTR("\n{\"level\":\"%S\",\"msg\":\""), status_level_pgmstr(level));
+ printf_P(PSTR("\n{\"level\":\"%"PRPSTR"\", \"msg\":\""),
+ status_level_pgmstr(level));
// Message
if (msg) {
if (code) printf_P(PSTR(", \"code\": %d"), code);
// Location
- if (location) printf_P(PSTR(", \"where\": \"%S\""), location);
+ if (location) printf_P(PSTR(", \"where\": \"%"PRPSTR"\""), location);
putchar('}');
putchar('\n');
for (int i = 0; i < STAT_MAX; i++) {
if (i) putchar(',');
putchar('\n');
- printf_P(PSTR(" \"%d\": \"%S\""), i, status_to_pgmstr(i));
+ printf_P(PSTR(" \"%d\": \"%"PRPSTR"\""), i, status_to_pgmstr(i));
}
putchar('\n');
/// Alarm state; send an exception report and stop processing input
-stat_t status_alarm(const char *location, stat_t code) {
- status_message_P(location, STAT_LEVEL_ERROR, code, 0);
+stat_t status_alarm(const char *location, stat_t code, const char *msg) {
+ status_message_P(location, STAT_LEVEL_ERROR, code, msg);
estop_trigger(code);
while (!usart_tx_empty()) continue;
return code;
#pragma once
-#include <avr/pgmspace.h>
+#include "pgmspace.h"
// RITORNO is a handy way to provide exception returns
void status_help();
/// Enter alarm state. returns same status code
-stat_t status_alarm(const char *location, stat_t status);
+stat_t status_alarm(const char *location, stat_t status, const char *msg);
#define TO_STRING(x) _TO_STRING(x)
#define _TO_STRING(x) #x
#define STATUS_ERROR(CODE, MSG, ...) \
STATUS_MESSAGE(STAT_LEVEL_ERROR, CODE, MSG, ##__VA_ARGS__)
-#define ALARM(CODE) status_alarm(STATUS_LOCATION, CODE)
-#define ASSERT(COND) do {if (!(COND)) ALARM(STAT_INTERNAL_ERROR);} while (0)
+#define ALARM(CODE) status_alarm(STATUS_LOCATION, CODE, 0)
+#define ASSERT(COND) \
+ do { \
+ if (!(COND)) \
+ status_alarm(STATUS_LOCATION, STAT_INTERNAL_ERROR, PSTR(#COND)); \
+ } while (0)
+
+
+#ifdef DEBUG
+#define DEBUG_CALL(FMT, ...) \
+ printf("%s(" FMT ")\n", __FUNCTION__, ##__VA_ARGS__)
+#else // DEBUG
+#define DEBUG_CALL(...)
+#endif // DEBUG
#include "util.h"
-
+#include <stdint.h>
/// Fast inverse square root originally from Quake III Arena code. Original
float invsqrt(float number) {
const float threehalfs = 1.5F;
- float x2 = number * 0.5;
+ float x2 = number * 0.5F;
float y = number;
- long i = *(long *)&y; // evil floating point bit level hacking
+ int32_t i = *(int32_t *)&y; // evil floating point bit level hacking
i = 0x5f3759df - (i >> 1); // what the fuck?
y = *(float *)&i;
y = y * (threehalfs - x2 * y * y); // 1st iteration
y = y * (threehalfs - x2 * y * y); // 2nd iteration, this can be removed
- return y; //isnan(y) ? 1.0 / sqrt(number) : y;
+ return y;
}
#define copy_vector(d, s) memcpy(d, s, sizeof(d))
// Math utilities
-inline float min(float a, float b) {return a < b ? a : b;}
-inline float max(float a, float b) {return a < b ? b : a;}
-inline float min3(float a, float b, float c) {return min(min(a, b), c);}
-inline float max3(float a, float b, float c) {return max(max(a, b), c);}
-inline float min4(float a, float b, float c, float d)
+inline static float min(float a, float b) {return a < b ? a : b;}
+inline static float max(float a, float b) {return a < b ? b : a;}
+inline static float min3(float a, float b, float c) {return min(min(a, b), c);}
+inline static float max3(float a, float b, float c) {return max(max(a, b), c);}
+inline static float min4(float a, float b, float c, float d)
{return min(min(a, b), min(c, d));}
-inline float max4(float a, float b, float c, float d)
+inline static float max4(float a, float b, float c, float d)
{return max(max(a, b), max(c, d));}
float invsqrt(float number);
+#ifndef __AVR__
+inline static double square(double x) {return x * x;}
+#endif
+
// Floating-point utilities
#define EPSILON 0.00001 // allowable rounding error for floats
-inline bool fp_EQ(float a, float b) {return fabs(a - b) < EPSILON;}
-inline bool fp_NE(float a, float b) {return fabs(a - b) > EPSILON;}
-inline bool fp_ZERO(float a) {return fabs(a) < EPSILON;}
-inline bool fp_NOT_ZERO(float a) {return !fp_ZERO(a);}
-inline bool fp_FALSE(float a) {return fp_ZERO(a);}
-inline bool fp_TRUE(float a) {return !fp_ZERO(a);}
+inline static bool fp_EQ(float a, float b) {return fabs(a - b) < EPSILON;}
+inline static bool fp_NE(float a, float b) {return fabs(a - b) > EPSILON;}
+inline static bool fp_ZERO(float a) {return fabs(a) < EPSILON;}
+inline static bool fp_FALSE(float a) {return fp_ZERO(a);}
+inline static bool fp_TRUE(float a) {return !fp_ZERO(a);}
// Constants
#define MM_PER_INCH 25.4
#include "status.h"
#include "hardware.h"
#include "config.h"
+#include "pgmspace.h"
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <ctype.h>
#include <math.h>
-
-#include <avr/pgmspace.h>
+#include <inttypes.h>
typedef uint8_t flags_t;
// Program string
static void var_print_pstring(pstring s) {
- printf_P(PSTR("\"%S\""), s);
+ printf_P(PSTR("\"%"PRPSTR"\""), s);
}
int vars_parser(char *vars) {
- if (!*vars || !*vars++ == '{') return STAT_OK;
+ if (!*vars || *vars != '{') return STAT_OK;
+ vars++;
while (*vars) {
while (isspace(*vars)) vars++;
void vars_print_help() {
- static const char fmt[] PROGMEM = " $%-5s %-20S %-16S %S\n";
+ static const char fmt[] PROGMEM =
+ " $%-5s %-20"PRPSTR" %-16"PRPSTR" %"PRPSTR"\n";
// Save and disable watchdog
uint8_t wd_state = hw_disable_watchdog();
--- /dev/null
+TESTS=planner-test
+
+PLANNER_TEST_SRC = gcode_parser.c machine.c status.c util.c axis.c report.c \
+ homing.c probing.c command.c vars.c
+PLANNER_TEST_SRC := $(patsubst %,../src/%,$(PLANNER_TEST_SRC))
+PLANNER_TEST_SRC += $(wildcard ../src/plan/*.c) planner-test.c hal.c
+
+CFLAGS = -I../src -Wall -Werror -DDEBUG -g -std=gnu99
+CFLAGS += -MD -MP -MT $@ -MF .dep/$(@F).d
+CFLAGS += -DF_CPU=320000000
+LDFLAGS = -lm
+
+all: $(TESTS)
+
+planner-test: $(PLANNER_TEST_SRC)
+ gcc -o $@ $(PLANNER_TEST_SRC) $(CFLAGS) $(LDFLAGS)
+
+# Clean
+tidy:
+ rm -f $(shell find -name \*~ -o -name \#\*)
+
+clean: tidy
+ rm -rf $(TESTS)
+
+.PHONY: tidy clean all
+
+# Dependencies
+-include $(shell mkdir -p .dep) $(wildcard .dep/*)
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 <http://www.gnu.org/licenses/>.
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "status.h"
+#include "spindle.h"
+#include "i2c.h"
+#include "cpp_magic.h"
+#include "plan/buffer.h"
+
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+
+typedef uint8_t flags_t;
+typedef const char *string;
+typedef const char *pstring;
+
+
+#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \
+ TYPE get_##NAME(IF(INDEX)(int index)) __attribute__((weak)); \
+ TYPE get_##NAME(IF(INDEX)(int index)) { \
+ DEBUG_CALL(); \
+ return 0; \
+ } \
+ IF(SET) \
+ (void set_##NAME(IF(INDEX)(int index,) TYPE value) __attribute__((weak)); \
+ void set_##NAME(IF(INDEX)(int index,) TYPE value) { \
+ DEBUG_CALL(); \
+ })
+
+#include "vars.def"
+#undef VAR
+
+
+void command_mreset(int argc, char *argv[]) {}
+void command_home(int argc, char *argv[]) {}
+void i2c_set_read_callback(i2c_read_cb_t cb) {}
+void print_status_flags(uint8_t flags) {DEBUG_CALL();}
+uint8_t hw_disable_watchdog() {return 0;}
+void hw_restore_watchdog(uint8_t state) {}
+
+
+bool estop = false;
+
+
+void estop_trigger(stat_t reason) {
+ DEBUG_CALL("%s", status_to_pgmstr(reason));
+ mp_queue_dump();
+ estop = true;
+ abort();
+}
+
+
+void estop_clear() {
+ DEBUG_CALL();
+ estop = false;
+}
+
+
+bool estop_triggered() {return estop;}
+
+
+void hw_request_hard_reset() {
+ DEBUG_CALL();
+ exit(0);
+}
+
+
+bool usart_tx_empty() {return true;}
+bool usart_tx_full() {return false;}
+
+
+char *usart_readline() {
+ static char *cmd = 0;
+
+ if (cmd) {
+ free(cmd);
+ cmd = 0;
+ }
+
+ size_t n = 0;
+ if (getline(&cmd, &n, stdin) == -1) {
+ free(cmd);
+ cmd = 0;
+ }
+
+ return cmd;
+}
+
+
+void coolant_init() {}
+
+
+void coolant_set_mist(bool x) {
+ DEBUG_CALL("%s", x ? "true" : "false");
+}
+
+
+void coolant_set_flood(bool x) {
+ DEBUG_CALL("%s", x ? "true" : "false");
+}
+
+
+void spindle_init() {}
+
+
+void spindle_set_speed(float speed) {
+ DEBUG_CALL("%f", speed);
+}
+
+
+void spindle_set_mode(spindle_mode_t mode) {
+ DEBUG_CALL("%d", mode);
+}
+
+
+void motor_set_encoder(int motor, float encoder) {
+ DEBUG_CALL("%d, %f", motor, encoder);
+}
+
+
+bool switch_is_active(int index) {
+ DEBUG_CALL("%d", index);
+ return false;
+}
+
+
+bool switch_is_enabled(int index) {
+ DEBUG_CALL("%d", index);
+ return false;
+}
+
+
+static uint32_t ticks = 0;
+
+
+uint32_t rtc_get_time() {return ticks;}
+
+
+bool rtc_expired(uint32_t t) {
+ return true;
+ return 0 <= (int32_t)(ticks - t);
+}
+
+
+bool motor_is_enabled(int motor) {return true;}
+int motor_get_axis(int motor) {return motor;}
+
+
+#define MICROSTEPS 32
+#define TRAVEL_REV 5
+#define STEP_ANGLE 1.8
+
+
+float motor_get_steps_per_unit(int motor) {
+ return 360 * MICROSTEPS / TRAVEL_REV / STEP_ANGLE;
+}
+
+
+int32_t motor_get_encoder(int motor) {
+ DEBUG_CALL("%d", motor);
+ return 0;
+}
+
+
+void motor_end_move(int motor) {
+ DEBUG_CALL("%d", motor);
+}
+
+
+bool st_is_busy() {return false;}
+
+
+float square(float x) {return x * x;}
+
+
+stat_t st_prep_line(float time, const float target[], const int32_t error[]) {
+ DEBUG_CALL("%f, (%f, %f, %f, %f), (%d, %d, %d, %d)",
+ time, target[0], target[1], target[2], target[3],
+ error[0], error[1], error[2], error[3]);
+
+ static float position[4] = {0};
+ float dist = 0;
+
+ for (int i = 0; i < 4; i++) {
+ dist += square((target[i] - position[i]) / motor_get_steps_per_unit(i));
+ position[i] = target[i];
+ }
+
+ dist = sqrt(dist);
+
+ float velocity = dist / time;
+
+ printf("%0.10f, %0.10f, %0.10f\n", velocity, dist, time);
+
+ return STAT_OK;
+}
+
+
+void st_prep_dwell(float seconds) {
+ DEBUG_CALL("%f", seconds);
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 <http://www.gnu.org/licenses/>.
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "axis.h"
+#include "machine.h"
+#include "command.h"
+#include "plan/planner.h"
+#include "plan/exec.h"
+#include "plan/state.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+
+
+int main(int argc, char *argv[]) {
+ mp_init(); // motion planning
+ machine_init(); // gcode machine
+ for (int i = 0; i < 4; i++) axis_set_motor(i, i);
+
+ stat_t status = STAT_OK;
+
+ while (!feof(stdin)) {
+ mp_state_callback();
+ command_callback();
+ }
+
+ while (true) {
+ status = mp_exec_move();
+ printf("EXEC: %s\n", status_to_pgmstr(status));
+
+ switch (status) {
+ case STAT_NOOP: break; // No command executed
+ case STAT_EAGAIN: continue; // No command executed, try again
+
+ case STAT_OK: // Move executed
+ //if (!st.move_queued) ALARM(STAT_EXPECTED_MOVE); // No move was queued
+ //st.move_queued = false;
+ //st.move_ready = true;
+ continue;
+
+ default:
+ printf("ERROR: %s\n", status_to_pgmstr(status));
+ }
+
+ break;
+ }
+
+ printf("STATE: %s\n", mp_get_state_pgmstr(mp_get_state()));
+
+ return status;
+}
projects / bbctrl-firmware / commitdiff