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;
}
-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;
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;
}
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,
#define SYNC_QUEUE_SIZE 4096
#define EXEC_FILL_TARGET 8
#define EXEC_DELAY 250 // ms
+#define JOG_STOPPING_UNDERSHOOT 1 // % of stopping distance
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;
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
// 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;
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;
}
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;
--- /dev/null
+#!/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()
projects / bbctrl-firmware / commitdiff