#define STEP_TIMER_ENABLE TC_CLKSEL_DIV4_gc
#define STEP_TIMER_DIV 4
#define STEP_TIMER_FREQ (F_CPU / STEP_TIMER_DIV)
-#define STEP_TIMER_POLL (STEP_TIMER_FREQ * 0.001)
+#define STEP_TIMER_POLL ((uint16_t)(STEP_TIMER_FREQ * 0.001))
#define STEP_TIMER_WGMODE TC_WGMODE_NORMAL_gc // count to TOP & rollover
#define STEP_TIMER_ISR TCC0_OVF_vect
#define STEP_TIMER_INTLVL TC_OVFINTLVL_HI_gc
}
-void motor_prep_move(int motor, int32_t target) {
+void motor_prep_move(int motor, float time, int32_t target) {
motor_t *m = &motors[motor];
// Validate input
// Find the fastest clock rate that will fit the required number of steps.
// Note, clock toggles step line so we need two clocks per step.
- uint24_t ticks_per_step = SEGMENT_CLOCKS / half_steps;
- if (SEGMENT_CLOCKS % half_steps) ticks_per_step++; // Round up
+ uint24_t seg_clocks = time * F_CPU * 60;
+ uint24_t ticks_per_step = seg_clocks / half_steps + 1; // Round up
if (ticks_per_step < 0xffff) m->timer_clock = TC_CLKSEL_DIV1_gc;
else if (ticks_per_step < 0x1ffff) m->timer_clock = TC_CLKSEL_DIV2_gc;
else if (ticks_per_step < 0x3ffff) m->timer_clock = TC_CLKSEL_DIV4_gc;
// Note, we rely on the fact that TC_CLKSEL_DIV1_gc through TC_CLKSEL_DIV8_gc
// equal 1, 2, 3 & 4 respectively.
- m->timer_period = ticks_per_step >> (m->timer_clock - 1);
- if (ticks_per_step & ((1 << (m->timer_clock - 1)) - 1))
- m->timer_period++; // Round up
+ m->timer_period = (ticks_per_step >> (m->timer_clock - 1)) + 1; // Round up
if (!m->timer_period || !half_steps) m->timer_clock = 0;
// Compute power from axis max velocity
const float max_step_vel = m->steps_per_unit * axis_get_velocity_max(m->axis);
- const float power = half_steps / (max_step_vel * SEGMENT_TIME * 2);
+ const float power = half_steps / (max_step_vel * time * 2);
drv8711_set_power(motor, power);
// Power motor
void motor_end_move(int motor);
void motor_load_move(int motor);
-void motor_prep_move(int motor, int32_t target);
+void motor_prep_move(int motor, float time, int32_t target);
mp_kinematics(travel, steps);
// Queue segment
- st_prep_line(steps);
+ st_prep_line(time, steps);
return STAT_EAGAIN;
}
uint24_t segment_count; // count of running segments
uint24_t segment; // current segment
+ float segment_time;
float segment_velocity; // computed velocity for segment
float segment_start[AXES];
float segment_delta;
ASSERT(Vi || Vt);
// len / avg. velocity
- float move_time = 2 * length / (Vi + Vt); // in mins
- float segments = ceil(move_time / SEGMENT_TIME);
- ex.segment_count = round(segments);
+ const float move_time = 2 * length / (Vi + Vt); // in mins
+ const float segments = round(move_time / SEGMENT_TIME);
+ ex.segment_count = segments;
+ ex.segment_time = move_time / segments; // in mins
ex.segment = 0;
ex.segment_dist = 0;
// Compute quintic Bezier curve
ex.segment_velocity =
velocity_curve(Vi, Vt, ex.segment * ex.segment_delta);
- ex.segment_dist += ex.segment_velocity * SEGMENT_TIME;
+ ex.segment_dist += ex.segment_velocity * ex.segment_time;
}
// Avoid overshoot
}
mp_runtime_set_velocity(ex.segment_velocity);
- RITORNO(mp_runtime_move_to_target(target));
+ RITORNO(mp_runtime_move_to_target(ex.segment_time, target));
// Return EAGAIN to continue or OK if this segment is done
return ex.segment < ex.segment_count ? STAT_EAGAIN : STAT_OK;
// Set velocity and target
mp_runtime_set_velocity(sqrt(velocity_sqr));
- stat_t status = mp_runtime_move_to_target(target);
+ stat_t status = mp_runtime_move_to_target(SEGMENT_TIME, target);
if (status != STAT_OK) return status;
return STAT_EAGAIN;
* Classes of moves:
*
* Requested-Fit - The move has sufficient length to achieve the target
- * velocity (cruise velocity). I.e it will accommodate the acceleration /
- * deceleration profile in the given length.
+ * velocity. I.e it will accommodate the acceleration / deceleration
+ * profile in the given length.
*
* Rate-Limited-Fit - The move does not have sufficient length to achieve
- * target velocity. In this case the cruise velocity will be set lower than
- * the requested velocity (incoming bf->cruise_velocity). The entry and
- * exit velocities are satisfied.
+ * target velocity. In this case, the cruise velocity will be lowered.
+ * The entry and exit velocities are satisfied.
*
* Degraded-Fit - The move does not have sufficient length to transition from
- * the entry velocity to the exit velocity in the available length. These
+ * the entry velocity to the exit velocity in the available length. These
* velocities are not negotiable, so a degraded solution is found.
*
* In worst cases, the move cannot be executed as the required execution
*
* Rate-Limited cases - Ve and Vx can be satisfied but Vt cannot:
*
- * HT (Ve=Vx)<Vt symmetric case. Split the length and compute Vt.
- * HT' (Ve!=Vx)<Vt asymmetric case. Find H and T by successive
+ * HT (Ve=Vx)<Vt symmetric case. Split the length and compute Vt.
+ * HT' (Ve!=Vx)<Vt asymmetric case. Find H and T by successive
* approximation.
- * HBT' body length < min body length - treated as an HT case
- * H' body length < min body length - subsume body into head length
- * T' body length < min body length - subsume body into tail length
+ * HBT' body length < min body length treated as an HT case
+ * H' body length < min body length subsume body into head length
+ * T' body length < min body length subsume body into tail length
*
* Degraded fit cases - line is too short to satisfy both Ve and Vx:
*
* H" Ve<Vx Ve is degraded (velocity step). Vx is met
* T" Ve>Vx Ve is degraded (velocity step). Vx is met
* B" <short> line is very short but drawable; is treated as a
- * body only
+ * body only.
* F <too short> force fit: This block is slowed down until it can
- * be executed
+ * be executed.
*
* Note: The order of the cases/tests in the code is important. Start with
* the shortest cases first and work up. Not only does this simplify the order
if (!bf->length) return;
// F case: Block is too short - run time < minimum segment time
- // Force block into a single segment body with limited velocities
+ // Force block into a single segment body with limited velocities.
// Accept the entry velocity, limit the cruise, and go for the best exit
// velocity you can get given the delta_vmax (maximum velocity slew).
-
float naive_move_time =
2 * bf->length / (bf->entry_velocity + bf->exit_velocity); // average
/// Set runtime position for a single axis
-void mp_runtime_set_axis_position(uint8_t axis, const float position) {
+void mp_runtime_set_axis_position(uint8_t axis, float position) {
rt.position[axis] = position;
report_request();
}
float *mp_runtime_get_position() {return rt.position;}
-void mp_runtime_set_position(float position[]) {
+void mp_runtime_set_position(const float position[]) {
copy_vector(rt.position, position);
report_request();
}
/// Segment runner
-stat_t mp_runtime_move_to_target(float target[]) {
+stat_t mp_runtime_move_to_target(float time, const float target[]) {
ASSERT(isfinite(target[0]) && isfinite(target[1]) &&
isfinite(target[2]) && isfinite(target[3]));
mp_kinematics(target, steps);
// Call the stepper prep function
- RITORNO(st_prep_line(steps));
+ RITORNO(st_prep_line(time, steps));
// Update positions
mp_runtime_set_position(target);
void mp_runtime_set_steps_from_position();
-void mp_runtime_set_axis_position(uint8_t axis, const float position);
+void mp_runtime_set_axis_position(uint8_t axis, float position);
float mp_runtime_get_axis_position(uint8_t axis);
float *mp_runtime_get_position();
-void mp_runtime_set_position(float position[]);
+void mp_runtime_set_position(const float position[]);
float mp_runtime_get_work_position(uint8_t axis);
void mp_runtime_set_work_offsets(float offset[]);
-stat_t mp_runtime_move_to_target(float target[]);
+stat_t mp_runtime_move_to_target(float time, const float target[]);
bool move_queued; // prepped move queued
move_type_t move_type;
float prep_dwell;
+ uint16_t clock_period;
} stepper_t;
// Setup step timer
TIMER_STEP.CTRLB = STEP_TIMER_WGMODE; // waveform mode
TIMER_STEP.INTCTRLA = STEP_TIMER_INTLVL; // interrupt mode
- TIMER_STEP.PER = SEGMENT_PERIOD; // timer rate
+ TIMER_STEP.PER = STEP_TIMER_POLL; // timer rate
TIMER_STEP.CTRLA = STEP_TIMER_ENABLE; // start step timer
+
+ st.clock_period = STEP_TIMER_POLL;
}
return;
}
+ // New clock period
+ TIMER_STEP.PER = st.clock_period;
+
// Dwell
if (0 < st.dwell) {
- st.dwell -= SEGMENT_SEC;
+ st.dwell -= 0.001; // 1ms
return;
} else st.dwell = 0;
st.move_type = MOVE_TYPE_NULL;
st.prep_dwell = 0; // clear dwell
st.move_ready = false; // flip the flag back
+ st.clock_period = STEP_TIMER_POLL;
// Request next move if not currently in a dwell. Requesting the next move
// may power up motors and the motors should not be powered up during a dwell.
* Steps are fractional. Their sign indicates direction. Motors not in the
* move have 0 steps.
*/
-stat_t st_prep_line(const float target[]) {
+stat_t st_prep_line(float time, const float target[]) {
// Trap conditions that would prevent queueing the line
ASSERT(!st.move_ready);
// Setup segment parameters
st.move_type = MOVE_TYPE_LINE;
+ st.clock_period = round(time * STEP_TIMER_FREQ * 60);
// Prepare motor moves
for (int motor = 0; motor < MOTORS; motor++) {
ASSERT(isfinite(target[motor]));
- motor_prep_move(motor, round(target[motor]));
+ motor_prep_move(motor, time, round(target[motor]));
}
st.move_queued = true; // signal prep buffer ready (do this last)
void stepper_init();
void st_shutdown();
bool st_is_busy();
-stat_t st_prep_line(const float target[]);
+stat_t st_prep_line(float time, const float target[]);
void st_prep_dwell(float seconds);
planner-test: $(PLANNER_TEST_SRC)
gcc -o $@ $(PLANNER_TEST_SRC) $(CFLAGS) $(LDFLAGS)
+test: planner-test
+ ./$< < test.gc
+
+csv: planner-test
+ @$(MAKE) -s test | grep -E '^-?[0-9.]+,'
+
+plot: planner-test
+ @$(MAKE) -s csv | ./plot_velocity.py
+
# Clean
tidy:
rm -f $(shell find -name \*~ -o -name \#\*)
clean: tidy
rm -rf $(TESTS)
-.PHONY: tidy clean all
+.PHONY: tidy clean all test csv plot
# Dependencies
-include $(shell mkdir -p .dep) $(wildcard .dep/*)
}
-void motor_set_encoder(int motor, float encoder) {
- DEBUG_CALL("%d, %f", motor, encoder);
+void motor_set_position(int motor, int32_t position) {
+ DEBUG_CALL("%d, %d", motor, position);
}
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]);
-
- double dist = 0;
-
- for (int i = 0; i < 4; i++)
- dist +=
- square((target[i] - motor_position[i]) / motor_get_steps_per_unit(i));
-
- dist = sqrt(dist);
-
- double velocity = dist / time;
+stat_t st_prep_line(float time, const float target[]) {
+ DEBUG_CALL("%f, (%f, %f, %f, %f)",
+ time, target[0], target[1], target[2], target[3]);
for (int i = 0; i < MOTORS; i++)
motor_position[i] = target[i];
- printf("%0.10f, %0.10f, %0.10f, %0.10f, %0.10f, %0.10f\n",
- velocity, dist, time,
- motor_position[0], motor_position[1], motor_position[2]);
+ printf("%0.10f, %0.10f, %0.10f, %0.10f\n",
+ time, motor_position[0], motor_position[1], motor_position[2]);
return STAT_OK;
}
printf("STATE: %s\n", mp_get_state_pgmstr(mp_get_state()));
- return status;
+ return 0;
}
--- /dev/null
+$resume
+
+$0vm=10000
+$1vm=10000
+$0jm=50
+$1jm=50
+
+G0 X500Y500
+G0 X0Y0
+G0 X-500Y0
+G0 X-500Y-500
projects / bbctrl-firmware / commitdiff