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))
@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 $@ $<
--- /dev/null
+/******************************************************************************\
+
+ 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 <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 "SCurve.h"
+
+#include <math.h>
+
+
+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;}
--- /dev/null
+/******************************************************************************\
+
+ 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 <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
+
+
+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);
+};
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);
}
#include "util.h"
#include "exec.h"
#include "state.h"
-#include "scurve.h"
#include "command.h"
#include "config.h"
+#include "SCurve.h"
#include <stdbool.h>
#include <math.h>
#include <stdlib.h>
-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);
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;
}
if (state_get() != STATE_READY && state_get() != STATE_JOGGING)
return STAT_NOP;
- // Skip command code
+ // Skip over command code
cmd++;
// Get velocities
// 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;
}
#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 <math.h>
#include <string.h>
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);
}
}
// 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;
-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.]+,'
#include "spindle.h"
#include "i2c.h"
#include "type.h"
+#include "switch.h"
#include "cpp_magic.h"
#include <stdint.h>
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;
void hw_request_hard_reset() {DEBUG_CALL(); exit(0);}
-bool usart_tx_empty() {return true;}
+bool usart_tx_fill() {return 0;}
char *usart_readline() {
}
-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;
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
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) {}
--- /dev/null
+/******************************************************************************\
+
+ CAMotics is an Open-Source simulation and CAM software.
+ Copyright (C) 2011-2017 Joseph Coffland <joseph@cauldrondevelopment.com>
+
+ 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 <http://www.gnu.org/licenses/>.
+
+\******************************************************************************/
+
+#include "SCurve.h"
+
+#include <iostream>
+#include <vector>
+
+#include <sys/time.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <string.h>
+#include <math.h>
+
+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] << " <min soft limit> <max soft limit> "
+ << "<max velocity> <max acceleration> <max jerk>" << 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<SCurve> 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
+ }
+}
projects / bbctrl-firmware / commitdiff