From: Joseph Coffland Date: Thu, 8 Mar 2018 23:39:46 +0000 (-0800) Subject: More work on soft limited jogging X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=763106b425f7bd140a97ceea5ab687376d61c3b0;p=bbctrl-firmware More work on soft limited jogging --- diff --git a/src/avr/Makefile b/src/avr/Makefile index bf6bbe7..d1489da 100644 --- a/src/avr/Makefile +++ b/src/avr/Makefile @@ -43,8 +43,8 @@ FUSE4=0xff FUSE5=0xeb # SRC -SRC = $(wildcard src/*.c) $(wildcard src/plan/*.c) -OBJ = $(patsubst src/%.c,build/%.o,$(SRC)) +SRC = $(wildcard src/*.c) $(wildcard src/*.cpp) +OBJ = $(patsubst src/%.cpp,build/%.o,$(patsubst src/%.c,build/%.o,$(SRC))) JSON = vars command messages JSON := $(patsubst %,build/%.json,$(JSON)) @@ -60,6 +60,10 @@ build/%.o: src/%.c @mkdir -p $(shell dirname $@) $(CC) $(INCLUDES) $(CFLAGS) -c -o $@ $< +build/%.o: src/%.cpp + @mkdir -p $(shell dirname $@) + $(CC) $(INCLUDES) $(CFLAGS) -c -o $@ $< + build/%.o: src/%.S @mkdir -p $(shell dirname $@) $(CC) $(INCLUDES) $(CFLAGS) -c -o $@ $< diff --git a/src/avr/src/SCurve.cpp b/src/avr/src/SCurve.cpp new file mode 100644 index 0000000..88680b4 --- /dev/null +++ b/src/avr/src/SCurve.cpp @@ -0,0 +1,167 @@ +/******************************************************************************\ + + 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 "SCurve.h" + +#include + + +SCurve::SCurve(float maxV, float maxA, float maxJ) : + maxV(maxV), maxA(maxA), maxJ(maxJ), v(0), a(0), j(0) {} + + +unsigned SCurve::getPhase() const { + if (!v) return 0; + + if (0 < a) { + if (0 < j) return 1; + if (!j) return 2; + return 3; + } + + if (!a) return 4; + if (j < 0) return 5; + if (!j) return 6; + + return 7; +} + + +float SCurve::getStoppingDist() const {return stoppingDist(v, a, maxA, maxJ);} + + +float SCurve::next(float t, float targetV) { + // Compute next acceleration + float nextA = nextAccel(t, targetV, v, a, maxA, maxJ); + + // Compute next velocity + float deltaV = nextA * t; + if ((deltaV < 0 && targetV < v && v + deltaV < targetV) || + (0 < deltaV && v < targetV && targetV < v + deltaV)) { + nextA = (targetV - v) / t; + v = targetV; + + } else v += deltaV; + + // Compute jerk = delta accel / time + j = (nextA - a) / t; + a = nextA; + + return v; +} + + +float SCurve::stoppingDist(float v, float a, float maxA, float maxJ) { + // Already stopped + if (!v) return 0; + + // Handle negative velocity + if (v < 0) { + v = -v; + a = -a; + } + + float d = 0; + + // Compute distance and velocity change to accel = 0 + if (0 < a) { + // Compute distance to decrease accel to zero + float t = a / maxJ; + d += distance(t, v, a, -maxJ); + v += velocity(t, a, -maxJ); + + } else if (a < 0) { + // Compute distance to increase accel to zero + float t = a / -maxJ; + d -= distance(t, v, a, maxJ); + v -= velocity(t, a, maxJ); + } + + // Compute max deccel from zero accel + float maxDeccel = -sqrt(v * maxJ); + if (maxDeccel < -maxA) maxDeccel = -maxA; + + // Compute distance and velocity change to max deccel + float t = maxDeccel / -maxJ; + d += distance(t, v, 0, -maxJ); + float deltaV = velocity(t, 0, -maxJ); + v += deltaV; + + // Compute constant deccel period + if (-deltaV < v) { + float t = (v + deltaV) / -maxDeccel; + d += distance(t, v, maxDeccel, 0); + v += velocity(t, maxDeccel, 0); + } + + // Compute distance to zero vel + d += distance(t, v, maxDeccel, maxJ); + + return d; +} + + +float SCurve::nextAccel(float t, float targetV, float v, float a, float maxA, + float maxJ) { + bool increasing = v < targetV; + float deltaA = t * maxJ; + + if (increasing && a < -deltaA) + return a + deltaA; // negative accel, increasing speed + + if (!increasing && deltaA < a) + return a - deltaA; // positive accel, decreasing speed + + float deltaV = fabs(targetV - v); + float targetA = sqrt(2 * deltaV * maxJ); + if (maxA < targetA) targetA = maxA; + + if (increasing) { + if (targetA < a + deltaA) return targetA; + return a + deltaA; + + } else { + if (a - deltaA < -targetA) return -targetA; + return a - deltaA; + } +} + + +float SCurve::distance(float t, float v, float a, float j) { + // v * t + 1/2 * a * t^2 + 1/6 * j * t^3 + return t * (v + t * (0.5 * a + 1.0 / 6.0 * j * t)); +} + + +float SCurve::velocity(float t, float a, float j) { + // a * t + 1/2 * j * t^2 + return t * (a + 0.5 * j * t); +} + + +float SCurve::acceleration(float t, float j) {return j * t;} diff --git a/src/avr/src/SCurve.h b/src/avr/src/SCurve.h new file mode 100644 index 0000000..b06ae20 --- /dev/null +++ b/src/avr/src/SCurve.h @@ -0,0 +1,64 @@ +/******************************************************************************\ + + 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 + + +class SCurve { + float maxV; + float maxA; + float maxJ; + + float v; + float a; + float j; + +public: + SCurve(float maxV = 0, float maxA = 0, float maxJ = 0); + + float getMaxVelocity() const {return maxV;} + void setMaxVelocity(float v) {maxV = v;} + float getMaxAcceleration() const {return maxA;} + void setMaxAcceleration(float a) {maxA = a;} + float getMaxJerk() const {return maxJ;} + void setMaxJerk(float j) {maxJ = j;} + + float getVelocity() const {return v;} + float getAcceleration() const {return a;} + float getJerk() const {return j;} + + unsigned getPhase() const; + float getStoppingDist() const; + float next(float t, float targetV); + + static float stoppingDist(float v, float a, float maxA, float maxJ); + static float nextAccel(float t, float targetV, float v, float a, float maxA, + float maxJ); + static float distance(float t, float v, float a, float j); + static float velocity(float t, float a, float j); + static float acceleration(float t, float j); +}; diff --git a/src/avr/src/axis.c b/src/avr/src/axis.c index ee9da55..0dea287 100644 --- a/src/avr/src/axis.c +++ b/src/avr/src/axis.c @@ -62,7 +62,7 @@ char axis_get_char(int axis) { int axis_get_id(char axis) { const char *axes = "XYZABCUVW"; - char *ptr = strchr(axes, toupper(axis)); + const char *ptr = strchr(axes, toupper(axis)); return ptr == 0 ? -1 : (ptr - axes); } diff --git a/src/avr/src/jog.c b/src/avr/src/jog.c index a678820..7227569 100644 --- a/src/avr/src/jog.c +++ b/src/avr/src/jog.c @@ -31,9 +31,9 @@ #include "util.h" #include "exec.h" #include "state.h" -#include "scurve.h" #include "command.h" #include "config.h" +#include "SCurve.h" #include #include @@ -42,220 +42,62 @@ #include -typedef struct { - float delta; - float t; - bool changed; - - int sign; - float velocity; - float accel; - float next; - float initial; - float target; -} jog_axis_t; - - -typedef struct { +static struct { bool writing; - bool done; - - jog_axis_t axes[AXES]; -} jog_runtime_t; - - -static jog_runtime_t jr; - - -#if 0 -// Numeric version -static float _compute_deccel_dist(float vel, float accel, float jerk) { - float dist = 0; - float Ad = jerk * SEGMENT_TIME; // Delta accel - - while (true) { - // Compute next accel - float At2 = -jerk * vel; - if (accel * accel < At2) accel += Ad; - else accel -= Ad; - - // Compute next velocity - vel += accel * SEGMENT_TIME; - if (vel <= 0) break; - - // Compute distance traveled - dist += vel * SEGMENT_TIME; - } - - return dist; -} -#else - - -// Analytical version -static float _compute_deccel_dist(float vel, float accel, float maxA, - float jerk) { - // TODO Fix this function - - float dist = 0; - - // Compute distance to decrease accel to zero - if (0 < accel) { - float t = accel / jerk; - dist += scurve_distance(t, vel, accel, -jerk); - vel += scurve_velocity(t, accel, -jerk); - accel = 0; - } - - // At this point accel <= 0, aka a decceleration - - // Compute max deccel by applying the quadratic formula. - float t = accel / jerk; - float a = -1 / jerk; - float b = 2 * t; - float c = vel - 1.5 * t * accel; - float maxDeccel = (-b + sqrt(b * b - 4 * a * c)) / a * 0.5; - - // Limit decceleration - if (maxDeccel < -maxA) maxDeccel = -maxA; - - // Compute distance and velocity change to max deccel - if (maxDeccel < accel) { - float t = (maxDeccel - accel) / jerk; - dist += scurve_distance(t, vel, accel, -jerk); - vel += scurve_velocity(t, accel, -jerk); - accel = maxDeccel; - } - - // Compute max deltaV for remaining deccel - t = -accel / jerk; // Time to shed remaining accel - float deltaV = -scurve_velocity(t, accel, jerk); - - // Compute constant deccel period - if (deltaV < vel) { - float t = -(vel - deltaV) / accel; - dist += scurve_distance(t, vel, accel, 0); - vel += scurve_velocity(t, accel, 0); - } - - // Compute distance to zero vel - dist += scurve_distance(t, vel, accel, jerk); - - return dist; -} -#endif - - -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; - bool positive = 0 < dir; - - // Check if axis is homed - if (!axis_get_homed(axis)) return Vt; - - // Check if limits are enabled - float min = axis_get_soft_limit(axis, true); - float max = axis_get_soft_limit(axis, false); - if (min == max) return Vt; - - // 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 positive ? Vt : 0; - if (max <= position) return !positive ? Vt : 0; - - // Min velocity near limits - if (positive && max < position + 1) return MIN_VELOCITY; - if (!positive && position - 1 < min) return MIN_VELOCITY; - // Compute dist to deccel - float jerk = axis_get_jerk_max(axis); - float maxA = axis_get_accel_max(axis); - float deccelDist = _compute_deccel_dist(V, A, maxA, jerk); + SCurve scurves[AXES]; + float next[AXES]; + float targetV[AXES]; +} jr; - // Check if deccel distance will lead to exceeding a limit - if (positive && max <= position + deccelDist) return 0; - if (!positive && position - deccelDist <= min) return 0; - return Vt; -} - - -static float _compute_axis_velocity(int axis) { - jog_axis_t *a = &jr.axes[axis]; - - float V = fabs(a->velocity); - float Vt = fabs(a->target); - - // Apply soft limits - Vt = _soft_limit(axis, V, Vt, a->accel); - - // Check if velocity has reached its target - if (fp_EQ(V, Vt)) { - a->accel = 0; - return Vt; - } - - // Compute axis max accel and jerk - float jerk = axis_get_jerk_max(axis); - float maxA = axis_get_accel_max(axis); - - // Compute next accel - a->accel = scurve_next_accel(SEGMENT_TIME, V, Vt, a->accel, maxA, jerk); - - return V + a->accel * SEGMENT_TIME; -} - - -static bool _axis_velocity_target(int axis) { - jog_axis_t *a = &jr.axes[axis]; - - float Vn = a->next * axis_get_velocity_max(axis); - float Vi = a->velocity; - float Vt = a->target; +stat_t jog_exec() { + bool done = true; - if (MIN_VELOCITY < fabs(Vn)) jr.done = false; // Still jogging + // Compute per axis velocities and target positions + float target[AXES] = {0,}; + float velocity_sqr = 0; - if (!fp_ZERO(Vi) && (Vn < 0) != (Vi < 0)) - Vn = 0; // Plan to zero on sign change + for (int axis = 0; axis < AXES; axis++) { + if (!axis_is_enabled(axis)) continue; - if (fabs(Vn) < MIN_VELOCITY) Vn = 0; + // Load next velocity + if (!jr.writing) + jr.targetV[axis] = jr.next[axis] * axis_get_velocity_max(axis); - if (Vt == Vn) return false; // No change + float p = exec_get_axis_position(axis); + float vel = jr.scurves[axis].getVelocity(); + float targetV = jr.targetV[axis]; + float min = axis_get_soft_limit(axis, true); + float max = axis_get_soft_limit(axis, false); + bool softLimited = min != max && axis_get_homed(axis); - a->target = Vn; - if (Vn) a->sign = Vn < 0 ? -1 : 1; + // Apply soft limits, if enabled and homed + if (softLimited && MIN_VELOCITY < fabs(targetV)) { + float dist = jr.scurves[axis].getStoppingDist() * 1.01; - return true; // Velocity changed -} + if (vel < 0 && p - dist <= min) targetV = -MIN_VELOCITY; + if (0 < vel && max <= p + dist) targetV = MIN_VELOCITY; + } + // Compute next velocity + float v = jr.scurves[axis].next(SEGMENT_TIME, targetV); -stat_t jog_exec() { - // Load next velocity - jr.done = true; + // Don't overshoot soft limits + float deltaP = v * SEGMENT_TIME; + if (softLimited && 0 < deltaP && max < p + deltaP) p = max; + else if (softLimited && deltaP < 0 && p + deltaP < min) p = min; + else p += deltaP; - if (!jr.writing) - for (int axis = 0; axis < AXES; axis++) { - if (!axis_is_enabled(axis)) continue; - jr.axes[axis].changed = _axis_velocity_target(axis); - } + // Not done jogging if still moving + if (MIN_VELOCITY < fabs(v) || MIN_VELOCITY < fabs(targetV)) done = false; - float velocity_sqr = 0; - - // Compute per axis velocities - for (int axis = 0; axis < AXES; axis++) { - if (!axis_is_enabled(axis)) continue; - float V = _compute_axis_velocity(axis); - if (MIN_VELOCITY < V) jr.done = false; - velocity_sqr += square(V); - jr.axes[axis].velocity = V * jr.axes[axis].sign; + velocity_sqr += square(v); + target[axis] = p; } // Check if we are done - if (jr.done) { + if (done) { command_reset_position(); exec_set_velocity(0); exec_set_cb(0); @@ -264,26 +106,16 @@ stat_t jog_exec() { return STAT_NOP; // Done, no move executed } - // Compute target from velocity - float target[AXES]; - exec_get_position(target); - for (int axis = 0; axis < AXES; axis++) - target[axis] += jr.axes[axis].velocity * SEGMENT_TIME; - // Set velocity and target exec_set_velocity(sqrt(velocity_sqr)); - stat_t status = exec_move_to_target(SEGMENT_TIME, target); - if (status != STAT_OK) return status; - - return STAT_OK; + return exec_move_to_target(SEGMENT_TIME, target); } void jog_stop() { if (state_get() != STATE_JOGGING) return; jr.writing = true; - for (int axis = 0; axis < AXES; axis++) - jr.axes[axis].next = 0; + for (int axis = 0; axis < AXES; axis++) jr.next[axis] = 0; jr.writing = false; } @@ -293,7 +125,7 @@ stat_t command_jog(char *cmd) { if (state_get() != STATE_READY && state_get() != STATE_JOGGING) return STAT_NOP; - // Skip command code + // Skip over command code cmd++; // Get velocities @@ -304,18 +136,24 @@ stat_t command_jog(char *cmd) { // Check for end of command if (*cmd) return STAT_INVALID_ARGUMENTS; - // Reset - if (state_get() != STATE_JOGGING) memset(&jr, 0, sizeof(jr)); + // Start jogging + if (state_get() != STATE_JOGGING) { + memset(&jr, 0, sizeof(jr)); - jr.writing = true; - for (int axis = 0; axis < AXES; axis++) - jr.axes[axis].next = velocity[axis]; - jr.writing = false; + for (int axis = 0; axis < AXES; axis++) + if (axis_is_enabled(axis)) + jr.scurves[axis] = + SCurve(axis_get_velocity_max(axis), axis_get_accel_max(axis), + axis_get_jerk_max(axis)); - if (state_get() != STATE_JOGGING) { state_jogging(); exec_set_cb(jog_exec); } + // Set next velocities + jr.writing = true; + for (int axis = 0; axis < AXES; axis++) jr.next[axis] = velocity[axis]; + jr.writing = false; + return STAT_OK; } diff --git a/src/avr/src/line.c b/src/avr/src/line.c index b22f686..5516e16 100644 --- a/src/avr/src/line.c +++ b/src/avr/src/line.c @@ -29,10 +29,10 @@ #include "exec.h" #include "axis.h" #include "command.h" -#include "scurve.h" #include "state.h" #include "seek.h" #include "util.h" +#include "SCurve.h" #include #include @@ -76,17 +76,17 @@ static void _segment_target(float target[AXES], float d) { static float _segment_distance(float t) { - return l.iD + scurve_distance(t, l.iV, l.iA, l.jerk); + return l.iD + SCurve::distance(t, l.iV, l.iA, l.jerk); } static float _segment_velocity(float t) { - return l.iV + scurve_velocity(t, l.iA, l.jerk); + return l.iV + SCurve::velocity(t, l.iA, l.jerk); } static float _segment_accel(float t) { - return l.iA + scurve_acceleration(t, l.jerk); + return l.iA + SCurve::acceleration(t, l.jerk); } @@ -172,7 +172,7 @@ static stat_t _pause() { } // Compute new velocity, acceleration and travel distance - a = scurve_next_accel(t, v, 0, a, l.line.max_accel, j); + a = SCurve::nextAccel(t, 0, v, a, l.line.max_accel, j); v += a * t; l.dist += v * t; diff --git a/src/avr/test/Makefile b/src/avr/test/Makefile index 3504160..793c3e6 100644 --- a/src/avr/test/Makefile +++ b/src/avr/test/Makefile @@ -1,19 +1,24 @@ -TARGET=firmware-test +TARGETS = firmware-test jogsim FIRMWARE_TEST_SRC = status.c util.c axis.c report.c type.c exec.c base64.c \ - command.c commands.c vars.c state.c line.c scurve.c seek.c + command.c commands.c vars.c state.c line.c seek.c SCurve.cpp FIRMWARE_TEST_SRC := $(patsubst %,../src/%,$(FIRMWARE_TEST_SRC)) -FIRMWARE_TEST_SRC += $(wildcard ../src/plan/*.c) firmware-test.c hal.c +FIRMWARE_TEST_SRC += firmware-test.c hal.c -CFLAGS = -I../src -Wall -Werror -DDEBUG -g -std=gnu99 +JOGSIM_SRC = ../src/SCurve.cpp jogsim.cpp + +CFLAGS = -I../src -Wall -Werror -DDEBUG -g -std=gnu++98 CFLAGS += -MD -MP -MT $@ -MF .dep/$(@F).d CFLAGS += -DF_CPU=320000000 LDFLAGS = -lm -all: $(TARGET) +all: $(TARGETS) + +firmware-test: $(FIRMWARE_TEST_SRC) + g++ -o $@ $(FIRMWARE_TEST_SRC) $(CFLAGS) $(LDFLAGS) -$(TARGET): $(FIRMWARE_TEST_SRC) - gcc -o $@ $(FIRMWARE_TEST_SRC) $(CFLAGS) $(LDFLAGS) +jogsim: $(JOGSIM_SRC) + g++ -o $@ $(JOGSIM_SRC) $(CFLAGS) $(LDFLAGS) %.csv: %.gc firmware-test ./firmware-test < $< | grep -E '^-?[0-9.]+,' diff --git a/src/avr/test/hal.c b/src/avr/test/hal.c index 6a081b5..baeca5d 100644 --- a/src/avr/test/hal.c +++ b/src/avr/test/hal.c @@ -29,6 +29,7 @@ #include "spindle.h" #include "i2c.h" #include "type.h" +#include "switch.h" #include "cpp_magic.h" #include @@ -62,7 +63,7 @@ typedef const char *pstring; float square(float x) {return x * x;} void i2c_set_read_callback(i2c_read_cb_t cb) {} -void print_status_flags(uint8_t flags) {DEBUG_CALL();} +void print_status_flags(uint16_t flags) {DEBUG_CALL();} bool estop = false; @@ -82,7 +83,7 @@ bool estop_triggered() {return estop;} void hw_request_hard_reset() {DEBUG_CALL(); exit(0);} -bool usart_tx_empty() {return true;} +bool usart_tx_fill() {return 0;} char *usart_readline() { @@ -116,8 +117,9 @@ void st_set_position(int32_t position[AXES]) { } -bool switch_is_active(int index) {DEBUG_CALL("%d", index); return false;} -bool switch_is_enabled(int index) {DEBUG_CALL("%d", index); return false;} +bool switch_is_active(switch_id_t sw) {DEBUG_CALL("%d", sw); return false;} +bool switch_is_enabled(switch_id_t sw) {DEBUG_CALL("%d", sw); return false;} +void switch_set_callback(switch_id_t sw, switch_callback_t cb) {} static uint32_t ticks = 0; @@ -127,7 +129,9 @@ bool rtc_expired(uint32_t t) {return true;} bool motor_is_enabled(int motor) {return true;} int motor_get_axis(int motor) {return motor;} - +bool motor_get_homed(int motor) {return false;} +float motor_get_soft_limit(int motor, bool min) {return 0;} +void motor_set_position(int motor, float position) {} #define MICROSTEPS 32 #define TRAVEL_REV 5 @@ -161,7 +165,14 @@ stat_t st_prep_line(float time, const float target[]) { void st_prep_dwell(float seconds) {DEBUG_CALL("%f", seconds);} +void outputs_stop() {} +void jog_stop() {} + + // Command callbacks stat_t command_estop(char *cmd) {DEBUG_CALL(); return STAT_OK;} stat_t command_clear(char *cmd) {DEBUG_CALL(); return STAT_OK;} stat_t command_jog(char *cmd) {DEBUG_CALL(); return STAT_OK;} +stat_t command_input(char *cmd) {DEBUG_CALL(); return STAT_OK;} +unsigned command_input_size() {return 0;} +void command_input_exec(void *data) {} diff --git a/src/avr/test/jogsim.cpp b/src/avr/test/jogsim.cpp new file mode 100644 index 0000000..b048586 --- /dev/null +++ b/src/avr/test/jogsim.cpp @@ -0,0 +1,135 @@ +/******************************************************************************\ + + CAMotics is an Open-Source simulation and CAM software. + Copyright (C) 2011-2017 Joseph Coffland + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 2 of the License, or + (at your option) any later version. + + This program 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 General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +\******************************************************************************/ + +#include "SCurve.h" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; + + +ostream &operator<<(ostream &stream, const float v[4]) { + return + stream << '(' << v[0] << ',' << v[1] << ',' << v[2] << ',' << v[3] << ')'; +} + + +int main(int argc, char *argv[]) { + if (argc != 6) { + cout << "Usage: " << argv[0] << " " + << " " << endl; + return 1; + } + + float min = atof(argv[1]); + float max = atof(argv[2]); + float maxVel = atof(argv[3]); + float maxAccel = atof(argv[4]); + float maxJerk = atof(argv[5]); + + vector scurves; + for (unsigned axis = 0; axis < 4; axis++) + scurves.push_back(SCurve(maxVel, maxAccel, maxJerk)); + + float p[4]; + float targetV[4]; + float lastP[4]; + const float deltaT = 0.005 / 60; + const float coastV = 10; // mm/min + + const size_t maxLen = 1024; + char *line = new char[maxLen]; + + // Non-blocking input + fcntl(0, F_SETFL, fcntl(0, F_GETFL) | O_NONBLOCK); + + while (true) { + while (true) { + struct timeval tv = {0, 0}; + fd_set fds; + + FD_ZERO(&fds); + FD_SET(0, &fds); + + int ret = select(1, &fds, 0, 0, &tv); + if (ret == -1) return 0; + if (!ret) break; + + ssize_t len = getline(&line, (size_t *)&maxLen, stdin); + if (len < 0) break; + + const char *delims = ", \t\n\r"; + char *s = line; + + for (unsigned i = 0; i < 4; i++) { + char *token = strtok(s, delims); + if (!token) break; + targetV[i] = atof(token) * maxVel; + s = 0; + } + } + + float v[4], a[4], j[4], d[4]; + bool changed = false; + + for (unsigned axis = 0; axis < 4; axis++) { + float vel = scurves[axis].getVelocity(); + float targetVel = targetV[axis]; + + if (coastV < fabs(targetVel)) { + float dist = scurves[axis].getStoppingDist(); + + if (vel < 0 && p[axis] - dist <= min) targetVel = -coastV; + if (0 < vel && max <= p[axis] + dist) targetVel = coastV; + } + + v[axis] = scurves[axis].next(deltaT, targetVel); + a[axis] = scurves[axis].getAcceleration(); + j[axis] = scurves[axis].getJerk(); + d[axis] = scurves[axis].getStoppingDist(); + + float deltaP = v[axis] * deltaT; + if (0 < deltaP && max < p[axis] + deltaP) p[axis] = max; + else if (deltaP < 0 && p[axis] + deltaP < min) p[axis] = min; + else p[axis] += deltaP; + + if (lastP[axis] != p[axis]) { + changed = true; + lastP[axis] = p[axis]; + } + } + + if (changed) + cout << p << ' ' << targetV << ' ' << v << ' ' << a << ' ' << j << ' ' + << d << endl; + + usleep(5000); // 5ms + } +}