From 873c962aaec66e5dc252862ad3350d71e1bb0ed4 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Mon, 19 Mar 2018 17:31:20 -0700 Subject: [PATCH] Fully functional soft-limited jogging. --- src/avr/src/SCurve.cpp | 76 +++++++++++------------------------------ src/avr/src/SCurve.h | 3 +- src/avr/src/config.h | 1 + src/avr/src/jog.c | 26 ++++---------- src/avr/src/motor.c | 45 ++++++++++++------------ src/avr/test/jogsim.cpp | 2 +- src/avr/test/jogtest.py | 50 +++++++++++++++++++++++++++ 7 files changed, 101 insertions(+), 102 deletions(-) create mode 100755 src/avr/test/jogtest.py diff --git a/src/avr/src/SCurve.cpp b/src/avr/src/SCurve.cpp index 08fad3c..73bbe30 100644 --- a/src/avr/src/SCurve.cpp +++ b/src/avr/src/SCurve.cpp @@ -63,10 +63,9 @@ unsigned SCurve::getPhase() const { float SCurve::getStoppingDist() const {return stoppingDist(v, a, maxA, maxJ);} -float SCurve::next(float t, float targetV, float overrideJ) { +float SCurve::next(float t, float targetV) { // Compute next acceleration - float nextA = - nextAccel(t, targetV, v, a, maxA, isfinite(overrideJ) ? overrideJ : maxJ); + float nextA = nextAccel(t, targetV, v, a, maxA, maxJ); // Compute next velocity float deltaV = nextA * t; @@ -85,43 +84,6 @@ float SCurve::next(float t, float targetV, float overrideJ) { } -float SCurve::adjustedJerkForStoppingDist(float d) const { - // Only make adjustments in phase 7 - if (getPhase() != 7) return NAN; - - // Handle negative velocity - float v = this->v; - float a = this->a; - if (v < 0) { - v = -v; - a = -a; - } - - // Compute distance to zero vel - float actualD = distance(-a / maxJ, v, 0, maxJ); - const float fudge = 0.05; - if (actualD < d) return maxJ * (1 + fudge); - else if (d < actualD) return maxJ * (1 - fudge); - return maxJ; - - // Compute jerk to hit distance exactly - // - // S-curve distance formula - // d = vt + 1/2 at^2 + 1/6 jt^3 - // - // t = -a/j when a < 0 - // - // Substitute and simplify - // (d = -va/j + (a^3)/(2j^2) - (a^3)/(6j^2)) * j^2 - // dj^2 = -jva + 1/2 a^3 - 1/6 a^3 - // dj^2 + vaj - 1/3 a^3 = 0 - // - // Apply quadratic formula - // j = (-va - sqrt((va)^2 + 4/3 da^3)) / (2d) - return -a * (v + sqrt(v * v + 1.333333 * d * a)) / (2 * d); -} - - float SCurve::stoppingDist(float v, float a, float maxA, float maxJ) { // Already stopped if (!v) return 0; @@ -140,33 +102,33 @@ float SCurve::stoppingDist(float v, float a, float maxA, float maxJ) { 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); + a = 0; } - // Compute max deccel from zero accel - float maxDeccel = -sqrt(v * maxJ); + // Compute max deccel + float maxDeccel = -sqrt(v * maxJ + 0.5 * a * a); 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; + if (maxDeccel < a) { + float t = (a - maxDeccel) / maxJ; + d += distance(t, v, a, -maxJ); + v += velocity(t, a, -maxJ); + a = maxDeccel; + } + + // Compute velocity change over remaining accel + float deltaV = 0.5 * a * a / maxJ; // Compute constant deccel period - if (-deltaV < v) { - float t = (v + deltaV) / -maxDeccel; - d += distance(t, v, maxDeccel, 0); - v += velocity(t, maxDeccel, 0); + if (deltaV < v) { + float t = (v - deltaV) / -a; + d += distance(t, v, a, 0); + v += velocity(t, a, 0); } // Compute distance to zero vel - d += distance(t, v, maxDeccel, maxJ); + d += distance(-a / maxJ, v, a, maxJ); return d; } diff --git a/src/avr/src/SCurve.h b/src/avr/src/SCurve.h index bcdf9b4..d48786a 100644 --- a/src/avr/src/SCurve.h +++ b/src/avr/src/SCurve.h @@ -55,8 +55,7 @@ public: unsigned getPhase() const; float getStoppingDist() const; - float next(float t, float targetV, float overrideJ = NAN); - float adjustedJerkForStoppingDist(float d) 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, diff --git a/src/avr/src/config.h b/src/avr/src/config.h index 77b6bac..a8b64de 100644 --- a/src/avr/src/config.h +++ b/src/avr/src/config.h @@ -217,3 +217,4 @@ enum { #define SYNC_QUEUE_SIZE 4096 #define EXEC_FILL_TARGET 8 #define EXEC_DELAY 250 // ms +#define JOG_STOPPING_UNDERSHOOT 1 // % of stopping distance diff --git a/src/avr/src/jog.c b/src/avr/src/jog.c index db2a513..7a7537a 100644 --- a/src/avr/src/jog.c +++ b/src/avr/src/jog.c @@ -73,30 +73,16 @@ stat_t jog_exec() { bool softLimited = min != max && axis_get_homed(axis); // Apply soft limits, if enabled and homed - float jerk = jr.scurves[axis].getMaxJerk(); if (softLimited && MIN_VELOCITY < fabs(targetV)) { - float dist = jr.scurves[axis].getStoppingDist(); - - float targetDist = 0; - if (vel < 0 && p - dist <= min) { - targetV = -MIN_VELOCITY; - targetDist = p - min; - } - - if (0 < vel && max <= p + dist) { - targetV = MIN_VELOCITY; - targetDist = max - p; - } - - if (targetDist) { - float adjustedJ = - jr.scurves[axis].adjustedJerkForStoppingDist(targetDist); - if (isfinite(adjustedJ)) jerk = adjustedJ; - } + float dist = jr.scurves[axis].getStoppingDist() * + (1 + (JOG_STOPPING_UNDERSHOOT / 100.0)); + + 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, jerk); + float v = jr.scurves[axis].next(SEGMENT_TIME, targetV); // Don't overshoot soft limits float deltaP = v * SEGMENT_TIME; diff --git a/src/avr/src/motor.c b/src/avr/src/motor.c index 0db996d..5e804fb 100644 --- a/src/avr/src/motor.c +++ b/src/avr/src/motor.c @@ -158,9 +158,6 @@ void motor_init() { m->dma->DESTADDR1 = (((uintptr_t)&_dummy) >> 8) & 0xff; m->dma->DESTADDR2 = 0; - m->dma->TRFCNT = 0xffff; - m->dma->REPCNT = 0; - m->dma->CTRLB = 0; m->dma->CTRLA = DMA_CH_SINGLE_bm | DMA_CH_BURSTLEN_1BYTE_gc; // IO pins @@ -247,9 +244,6 @@ void motor_end_move(int motor) { // Get actual step count from DMA channel const int32_t steps = 0xffff - m->dma->TRFCNT; - // Disable DMA step counter - m->dma->CTRLA &= ~DMA_CH_ENABLE_bm; - // Accumulate encoder & compute error m->encoder += m->last_negative ? -steps : steps; m->error = m->commanded - m->encoder; @@ -259,29 +253,36 @@ void motor_end_move(int motor) { void motor_load_move(int motor) { motor_t *m = &motors[motor]; + // Clear move ASSERT(m->prepped); + m->prepped = false; motor_end_move(motor); // Set direction, compensating for polarity - SET_PIN(m->dir_pin, m->negative ^ m->reverse); - - if (m->timer_period) { - // Setup DMA step counter - m->dma->TRFCNT = 0xffff; - m->dma->CTRLA |= DMA_CH_ENABLE_bm; - - // Set clock and period - m->timer->PERBUF = m->timer_period; // Set next frequency - if (m->timer_period < m->timer->CNT) m->timer->CNT = m->timer_period - 1; - m->timer->CTRLA = TC_CLKSEL_DIV2_gc; // Start clock - m->last_negative = m->negative; - m->commanded = m->position; + const bool dir = m->negative ^ m->reverse; + if (dir != IN_PIN(m->dir_pin)) { + SET_PIN(m->dir_pin, dir); + // Make sure there is plenty of time between direction change and next step. + if (m->timer->CCA < m->timer->CNT) m->timer->CNT = m->timer->CCA + 1; + } - } else m->timer->CTRLA = 0; + if (!m->timer_period) return; // Leave clock stopped - // Clear move - m->prepped = false; + // Reset DMA step counter + m->dma->CTRLA &= ~DMA_CH_ENABLE_bm; + m->dma->TRFCNT = 0xffff; + m->dma->CTRLA |= DMA_CH_ENABLE_bm; + + // Note, it is important to start the clock, if it is stopped, before + // setting PERBUF so that PER is not updated immediately possibly + // interrupting the clock mid step or causing counter wrap around. + + // Set clock and period + m->timer->CTRLA = TC_CLKSEL_DIV2_gc; // Start clock + m->timer->PERBUF = m->timer_period; // Set next frequency + m->last_negative = m->negative; + m->commanded = m->position; } diff --git a/src/avr/test/jogsim.cpp b/src/avr/test/jogsim.cpp index b048586..a01af1b 100644 --- a/src/avr/test/jogsim.cpp +++ b/src/avr/test/jogsim.cpp @@ -104,7 +104,7 @@ int main(int argc, char *argv[]) { float targetVel = targetV[axis]; if (coastV < fabs(targetVel)) { - float dist = scurves[axis].getStoppingDist(); + float dist = scurves[axis].getStoppingDist() * 1.01; if (vel < 0 && p[axis] - dist <= min) targetVel = -coastV; if (0 < vel && max <= p[axis] + dist) targetVel = coastV; diff --git a/src/avr/test/jogtest.py b/src/avr/test/jogtest.py new file mode 100755 index 0000000..709156b --- /dev/null +++ b/src/avr/test/jogtest.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python3 + +import sys +import tornado.ioloop + +from inevent import InEvent, JogHandler +from inevent.Constants import * + + +class Handler(JogHandler): + def changed(self): + scale = 1.0 + if self.speed == 1: scale = 1.0 / 128.0 + if self.speed == 2: scale = 1.0 / 32.0 + if self.speed == 3: scale = 1.0 / 4.0 + + print(', '.join(list(map(lambda x: '%.3f' % (x * scale), self.axes)))) + sys.stdout.flush() + + +if __name__ == "__main__": + # Load config + config = { + "Logitech Logitech RumblePad 2 USB": { + "deadband": 0.1, + "axes": [ABS_X, ABS_Y, ABS_RZ, ABS_Z], + "dir": [1, -1, -1, 1], + "arrows": [ABS_HAT0X, ABS_HAT0Y], + "speed": [0x120, 0x121, 0x122, 0x123], + "lock": [0x124, 0x125], + }, + + "default": { + "deadband": 0.1, + "axes": [ABS_X, ABS_Y, ABS_RY, ABS_RX], + "dir": [1, -1, -1, 1], + "arrows": [ABS_HAT0X, ABS_HAT0Y], + "speed": [0x133, 0x130, 0x131, 0x134], + "lock": [0x136, 0x137], + } + } + + # Create ioloop + ioloop = tornado.ioloop.IOLoop.current() + + # Listen for input events + handler = Handler(config) + events = InEvent(ioloop, handler, types = "js kbd".split()) + + ioloop.start() -- 2.27.0