# Parsing, Queuing & Planning
- * command_callback()
- * gc_gcode_parser(const char *block)
- * _normalize_gcode_block(...)
- * _parse_gcode_block(const char *block)
- * _execute_gcode_block()
- * mach_*()
- * mach_straight_traverse() || mach_straight_feed() || mach_arc_feed()
- * mp_aline(const float target[], float feed,. . .)
- * _calc_jerk*()
- * _calc_move_time()
- * _calc_max_velocities()
- * _get_junction_vmax()
- * mp_plan()
- * mp_calculate_trapezoid()
- * mp_get_target_length()
- * mp_get_target_velocity()
- * mp_queue_push(buffer_cb_t cb, uint32_t time)
+ * main()
+ * command_callback()
+ * gc_gcode_parser(const char *block)
+ * _normalize_gcode_block(...)
+ * _parse_gcode_block(const char *block)
+ * _execute_gcode_block()
+ * mach_*()
+ * mach_straight_traverse/feed() || mach_arc_feed()
+ * mp_aline(const float target[], float feed,. . .)
+ * _calc_jerk*()
+ * _calc_move_time()
+ * _calc_max_velocities()
+ * _get_junction_vmax()
+ * mp_plan()
+ * mp_calculate_trapezoid()
+ * mp_get_target_length()
+ * mp_get_target_velocity()
+ * mp_queue_push(buffer_cb_t cb, uint32_t time)
# Execution
- * Stepper low-level ISR
+ * STEP_LOW_LEVEL_ISR
* mp_exec_move()
* mp_queue_get_head()
* mp_buffer->cb()
# Step Output
* STEP_TIMER_ISR
- * motor_end_move()
- * _request_exec_move()
- * Triggers Stepper low-level ISR
- * motor_load_move()
+ * _load_move()
+ * _end_move()
+ * motor_end_move()
+ * _request_exec_move()
+ * Triggers STEP_LOW_LEVEL_ISR
+ * motor_load_move()
+ * motor_end_move()
# Data flow
void axis_set_motor(int axis, int motor) {
motor_map[axis] = motor;
- axis_set_jerk_max(axis, axes[motor].jerk_max); // Init 1/jerk
+ axis_set_jerk_max(axis, axes[axis].jerk_max); // Init 1/jerk
}
#define M3_DMA_CH DMA.CH2
#define M4_DMA_CH DMA.CH3
-#define M1_DMA_VECT DMA_CH0_vect
-#define M2_DMA_VECT DMA_CH1_vect
-#define M3_DMA_VECT DMA_CH2_vect
-#define M4_DMA_VECT DMA_CH3_vect
-
#define M1_DMA_TRIGGER DMA_CH_TRIGSRC_TCD0_CCA_gc
#define M2_DMA_TRIGGER DMA_CH_TRIGSRC_TCE0_CCA_gc
#define M3_DMA_TRIGGER DMA_CH_TRIGSRC_TCF0_CCA_gc
#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
+#define STEP_LOW_LEVEL_ISR ADCB_CH0_vect
#define SEGMENT_USEC 5000.0 // segment time
#define SEGMENT_SEC (SEGMENT_USEC / 1000000.0)
#define SEGMENT_TIME (SEGMENT_SEC / 60.0)
-#define SEGMENT_CLOCKS (F_CPU * SEGMENT_SEC)
-#define SEGMENT_PERIOD (STEP_TIMER_FREQ * SEGMENT_SEC)
+#define SEGMENT_CLOCKS ((uint24_t)(F_CPU * SEGMENT_SEC))
+#define SEGMENT_PERIOD ((uint16_t)(STEP_TIMER_FREQ * SEGMENT_SEC))
// Huanyang settings
#define HUANYANG_PORT USARTD1
#define MOTOR_IDLE_TIMEOUT 0.25 // secs, motor off after this time
-//#define STEP_CORRECTION // Enable step correction
-#define MAX_STEP_CORRECTION 4 // In steps per segment
+#define MIN_HALF_STEP_CORRECTION 4
#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arcs
#define JUNCTION_DEVIATION 0.05 // default value, in mm
#define JUNCTION_ACCELERATION 100000 // centripetal corner accel
mp_state_callback();
mach_arc_callback(); // arc generation runs
home_callback();
- //mach_homing_callback(); // G28.2 continuation
+ //mach_homing_callback(); // G28.2 continuation TODO
mach_probe_callback(); // G38.2 continuation
}
command_callback(); // process next command
#include "plan/runtime.h"
#include "plan/calibrate.h"
+#include <util/delay.h>
+
#include <string.h>
#include <math.h>
#include <stdio.h>
typedef enum {
MOTOR_OFF,
MOTOR_IDLE, // motor stopped and may be partially energized
- MOTOR_ENERGIZING,
MOTOR_ACTIVE
} motor_power_state_t;
DMA_CH_t *dma;
uint8_t dma_trigger;
+ // Computed
+ float steps_per_unit;
+
// Runtime state
motor_power_state_t power_state; // state machine for managing motor power
uint32_t power_timeout;
motor_flags_t flags;
- bool active;
+ int32_t commanded;
+ int32_t encoder;
+ int16_t error;
+ bool last_negative;
+ uint8_t last_clock;
// Move prep
+ bool prepped;
uint8_t timer_clock;
uint16_t timer_period;
- uint16_t steps;
- bool clockwise;
+ bool negative;
int32_t position;
- float power;
} motor_t;
static uint8_t _dummy;
+static void _update_config(int motor) {
+ motor_t *m = &motors[motor];
+
+ m->steps_per_unit = 360.0 * m->microsteps / m->travel_rev / m->step_angle;
+}
+
+
void motor_init() {
// Enable DMA
DMA.CTRL = DMA_RESET_bm;
for (int motor = 0; motor < MOTORS; motor++) {
motor_t *m = &motors[motor];
+ _update_config(motor);
axis_set_motor(m->axis, motor);
// IO pins
- DIRSET_PIN(m->step_pin); // Output
- DIRSET_PIN(m->dir_pin); // Output
+ DIRSET_PIN(m->step_pin); // Output
+ DIRSET_PIN(m->dir_pin); // Output
// Setup motor timer
m->timer->CTRLB = TC_WGMODE_FRQ_gc | TC1_CCAEN_bm;
m->dma->DESTADDR1 = (((uintptr_t)&_dummy) >> 8) & 0xff;
m->dma->DESTADDR2 = 0;
+ m->dma->TRFCNT = 0xffff;
m->dma->REPCNT = 0;
- m->dma->CTRLB = DMA_CH_TRNINTLVL_HI_gc;
+ m->dma->CTRLB = 0;
m->dma->CTRLA = DMA_CH_SINGLE_bm | DMA_CH_BURSTLEN_1BYTE_gc;
drv8711_set_microsteps(motor, m->microsteps);
}
-float motor_get_steps_per_unit(int motor) {
- return 360 * motors[motor].microsteps / motors[motor].travel_rev /
- motors[motor].step_angle;
-}
-
-
-float motor_get_units_per_step(int motor) {
- return motors[motor].travel_rev * motors[motor].step_angle /
- motors[motor].microsteps / 360;
-}
-
-
-float _get_max_velocity(int motor) {
- return
- axis_get_velocity_max(motors[motor].axis) * motor_get_steps_per_unit(motor);
-}
-
-
+float motor_get_steps_per_unit(int motor) {return motors[motor].steps_per_unit;}
uint16_t motor_get_microsteps(int motor) {return motors[motor].microsteps;}
}
motors[motor].microsteps = microsteps;
+ _update_config(motor);
drv8711_set_microsteps(motor, microsteps);
}
void motor_set_position(int motor, int32_t position) {
//if (st_is_busy()) ALARM(STAT_INTERNAL_ERROR); TODO
- motors[motor].position = position;
+ motor_t *m = &motors[motor];
+
+ m->commanded = m->encoder = m->position = position << 1; // We use half steps
+ m->error = 0;
}
-int32_t motor_get_position(int motor) {return motors[motor].position;}
+int32_t motor_get_position(int motor) {
+ return motors[motor].position >> 1; // Convert from half to full steps
+}
/// returns true if motor is in an error state
// Fall through
case MOTOR_ALWAYS_POWERED:
- if (m->power_state != MOTOR_ACTIVE && m->power_state != MOTOR_ENERGIZING &&
- !motor_error(motor)) {
- _set_power_state(motor, MOTOR_ENERGIZING);
+ if (m->power_state != MOTOR_ACTIVE && !motor_error(motor)) {
// TODO is ~5ms enough time to enable the motor?
drv8711_set_state(motor, DRV8711_ACTIVE);
-
+ m->power_state = MOTOR_ACTIVE;
motor_driver_callback(motor); // TODO Shouldn't call this directly
}
break;
}
-bool motor_energizing() {
- for (int m = 0; m < MOTORS; m++)
- if (motors[m].power_state == MOTOR_ENERGIZING)
- return true;
-
- return false;
-}
-
-
void motor_driver_callback(int motor) {
motor_t *m = &motors[motor];
}
-static void _load_move(int motor) {
+void motor_end_move(int motor) {
motor_t *m = &motors[motor];
+ if (!m->timer->CTRLA) return;
+
// Stop clock
m->timer->CTRLA = 0;
- // Clear DMA interrupt flags
- m->dma->CTRLB |= DMA_CH_TRNIF_bm | DMA_CH_ERRIF_bm;
+ // Get actual step count from DMA channel
+ const int24_t half_steps = 0xffff - m->dma->TRFCNT;
- m->active = m->timer_period && m->timer_clock;
- if (!m->active) return;
+ // Accumulate encoder
+ m->encoder += m->last_negative ? -half_steps : half_steps;
+
+ // Compute error
+ m->error = m->commanded - m->encoder;
+}
- // Set direction
- if (m->clockwise) OUTCLR_PIN(m->dir_pin);
+
+void motor_load_move(int motor) {
+ motor_t *m = &motors[motor];
+
+ ASSERT(m->prepped);
+
+ motor_end_move(motor);
+
+ // Set direction, compensating for polarity
+ const bool clockwise = !(m->negative ^ m->reverse);
+ if (clockwise) OUTCLR_PIN(m->dir_pin);
else OUTSET_PIN(m->dir_pin);
- // Count steps with phony DMA transfer
- m->dma->TRFCNT = m->steps;
- m->dma->CTRLA |= DMA_CH_ENABLE_bm;
+ // Adjust clock count
+ if (m->last_clock) {
+ uint24_t count = m->timer->CNT;
+ int8_t freq_change = m->last_clock - m->timer_clock;
- // Set clock and period
- m->timer->CCA = m->timer_period; // Set step pulse period
- m->timer->CTRLA = m->timer_clock; // Set clock rate
-}
+ count <<= freq_change; // Adjust count
+ if (m->timer_period <= count) count -= m->timer_period;
+ if (m->timer_period <= count) count -= m->timer_period;
+ if (m->timer_period <= count) count = m->timer_period >> 1;
-ISR(M1_DMA_VECT) {_load_move(0);}
-ISR(M2_DMA_VECT) {_load_move(1);}
-ISR(M3_DMA_VECT) {_load_move(2);}
-ISR(M4_DMA_VECT) {_load_move(3);}
+ m->timer->CNT = count;
+ } else m->timer->CNT = m->timer_period >> 1;
-void motor_load_move(int motor) {
- if (!motors[motor].active) _load_move(motor);
+ // Reset DMA channel counter
+ m->dma->CTRLA &= ~DMA_CH_ENABLE_bm;
+ m->dma->TRFCNT = 0xffff;
+ m->dma->CTRLA |= DMA_CH_ENABLE_bm;
+
+ // Set clock and period
+ m->timer->CCA = m->timer_period; // Set frequency
+ m->timer->CTRLA = m->timer_clock; // Start or stop
+ m->last_clock = m->timer_clock; // Save clock value
+ m->timer_clock = 0; // Clear clock
+ m->last_negative = m->negative;
+ m->commanded = m->position;
+
+ // Clear move
+ m->prepped = false;
}
-stat_t motor_prep_move(int motor, int32_t target) {
+void motor_prep_move(int motor, int32_t target) {
motor_t *m = &motors[motor];
// Validate input
- if (motor < 0 || MOTORS < motor) return ALARM(STAT_INTERNAL_ERROR);
- if (isinf(target)) return ALARM(STAT_MOVE_TARGET_INFINITE);
- if (isnan(target)) return ALARM(STAT_MOVE_TARGET_NAN);
+ ASSERT(0 <= motor && motor < MOTORS);
+ ASSERT(!m->prepped);
+
+ // We count in half steps
+ target = target << 1;
// Compute travel in steps
- int32_t steps = target - m->position;
+ int24_t half_steps = target - m->position;
m->position = target;
- // Set direction, compensating for polarity
- bool negative = steps < 0;
- bool clockwise = !(negative ^ m->reverse);
+ // Error correction
+ int16_t correction = abs(m->error);
+ if (MIN_HALF_STEP_CORRECTION <= correction) {
+ // Allowed step correction is proportional to velocity
+ int24_t positive_half_steps = half_steps < 0 ? -half_steps : half_steps;
+ int16_t max_correction = (positive_half_steps >> 5) + 1;
+ if (max_correction < correction) correction = max_correction;
- // Positive steps from here on
- if (negative) steps = -steps;
+ if (m->error < 0) correction = -correction;
- // 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
- uint32_t ticks_per_step = SEGMENT_CLOCKS / 2 / steps;
- uint8_t timer_clock;
- if (ticks_per_step <= 0xffff) timer_clock = TC_CLKSEL_DIV1_gc;
- else if (ticks_per_step <= 0x1ffff) timer_clock = TC_CLKSEL_DIV2_gc;
- else if (ticks_per_step <= 0x3ffff) timer_clock = TC_CLKSEL_DIV4_gc;
- else if (ticks_per_step <= 0x7ffff) timer_clock = TC_CLKSEL_DIV8_gc;
- else timer_clock = 0; // Clock off, too slow
+ half_steps += correction;
+ m->error -= correction;
+ }
+
+ // Positive steps from here on
+ m->negative = half_steps < 0;
+ if (m->negative) half_steps = -half_steps;
+
+ // 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
+ 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;
+ else if (ticks_per_step < 0x7ffff) m->timer_clock = TC_CLKSEL_DIV8_gc;
+ else m->timer_clock = 0; // Clock off, too slow
// Note, we rely on the fact that TC_CLKSEL_DIV1_gc through TC_CLKSEL_DIV8_gc
// equal 1, 2, 3 & 4 respectively.
- uint16_t timer_period = ticks_per_step >> (timer_clock - 1);
+ 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
- // Compute power from axis velocity
- float power = steps / (_get_max_velocity(motor) * SEGMENT_TIME);
- drv8711_set_power(motor, m->power);
+ 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);
+ drv8711_set_power(motor, power);
// Power motor
switch (m->power_mode) {
case MOTOR_POWERED_ONLY_WHEN_MOVING:
- if (!steps) break; // Not moving
+ if (!m->timer_clock) break; // Not moving
// Fall through
case MOTOR_ALWAYS_POWERED: case MOTOR_POWERED_IN_CYCLE:
_update_power(motor);
// Queue move
- sei();
- m->clockwise = clockwise;
- m->steps = steps;
- m->timer_clock = timer_clock;
- m->timer_period = timer_period;
- m->power = power;
- cli();
-
- return STAT_OK;
+ m->prepped = true;
}
// Var callbacks
-char get_axis_name(int motor) {return axis_get_char(motors[motor].axis);}
+float get_step_angle(int motor) {return motors[motor].step_angle;}
-void set_axis_name(int motor, char axis) {
- int id = axis_get_id(axis);
- if (id != -1) motors[motor].axis = id;
+void set_step_angle(int motor, float value) {
+ motors[motor].step_angle = value;
+ _update_config(motor);
}
-float get_step_angle(int motor) {return motors[motor].step_angle;}
-void set_step_angle(int motor, float value) {motors[motor].step_angle = value;}
float get_travel(int motor) {return motors[motor].travel_rev;}
-void set_travel(int motor, float value) {motors[motor].travel_rev = value;}
+
+
+void set_travel(int motor, float value) {
+ motors[motor].travel_rev = value;
+ _update_config(motor);
+}
+
+
uint16_t get_microstep(int motor) {return motors[motor].microsteps;}
if (MOTORS <= motor || AXES <= value || value == motors[motor].axis) return;
axis_set_motor(motors[motor].axis, -1);
motors[motor].axis = value;
+ _update_config(motor);
axis_set_motor(value, motor);
}
+char get_axis_name(int motor) {return axis_get_char(motors[motor].axis);}
+
+
+void set_axis_name(int motor, char axis) {
+ int id = axis_get_id(axis);
+ if (id != -1) set_motor_axis(motor, id);
+}
+
+
uint8_t get_power_mode(int motor) {return motors[motor].power_mode;}
uint8_t get_status_strings(int m) {return get_status_flags(m);}
-int32_t get_encoder(int m) {return 0;}
+int32_t get_encoder(int m) {return motors[m].encoder;}
+int32_t get_error(int m) {return motors[m].error;}
// Command callback
void motor_set_stall_callback(int motor, stall_callback_t cb);
float motor_get_stall_homing_velocity(int motor);
float motor_get_steps_per_unit(int motor);
-float motor_get_units_per_step(int motor);
uint16_t motor_get_microsteps(int motor);
void motor_set_microsteps(int motor, uint16_t microsteps);
void motor_set_position(int motor, int32_t position);
bool motor_stalled(int motor);
void motor_reset(int motor);
-bool motor_energizing();
-
void motor_driver_callback(int motor);
stat_t motor_rtc_callback();
void motor_error_callback(int motor, motor_flags_t errors);
+void motor_end_move(int motor);
void motor_load_move(int motor);
-stat_t motor_prep_move(int motor, int32_t target);
+void motor_prep_move(int motor, int32_t target);
ex.segment_dist += ex.segment_velocity * SEGMENT_TIME;
}
+ // Avoid overshoot
+ if (length < ex.segment_dist) ex.segment_dist = length;
+
for (int axis = 0; axis < AXES; axis++)
target[axis] = ex.segment_start[axis] + ex.unit[axis] * ex.segment_dist;
}
// Setup step timer
TIMER_STEP.CTRLB = STEP_TIMER_WGMODE; // waveform mode
TIMER_STEP.INTCTRLA = STEP_TIMER_INTLVL; // interrupt mode
- TIMER_STEP.PER = STEP_TIMER_POLL; // timer idle rate
+ TIMER_STEP.PER = SEGMENT_PERIOD; // timer rate
TIMER_STEP.CTRLA = STEP_TIMER_ENABLE; // start step timer
}
/// Interrupt handler for calling move exec function.
/// ADC channel 0 triggered by load ISR as a "software" interrupt.
-ISR(ADCB_CH0_vect) {
+ISR(STEP_LOW_LEVEL_ISR) {
while (true) {
stat_t status = mp_exec_move();
if (st.requesting) return;
st.requesting = true;
- // Use ADC as a "software" interrupt to trigger next move exec
- ADCB_CH0_INTCTRL = ADC_CH_INTLVL_LO_gc; // LO level interrupt
+ // Use ADC as "software" interrupt to trigger next move exec as low interrupt.
+ ADCB_CH0_INTCTRL = ADC_CH_INTLVL_LO_gc;
ADCB_CTRLA = ADC_ENABLE_bm | ADC_CH0START_bm;
}
-/// Step timer interrupt routine
-/// Dequeue move and load into stepper struct
-ISR(STEP_TIMER_ISR) {
- // Dwell
- if (0 < st.dwell) {
- st.dwell -= SEGMENT_SEC;
- return;
- } else st.dwell = 0;
+static void _end_move() {
+ for (int motor = 0; motor < MOTORS; motor++)
+ motor_end_move(motor);
+}
- // Default clock rate
- TIMER_STEP.PER = STEP_TIMER_POLL;
+/// Dwell or dequeue and load next move.
+static void _load_move() {
+ // Check EStop
if (estop_triggered()) {
st.move_type = MOVE_TYPE_NULL;
+ _end_move();
return;
}
+ // Dwell
+ if (0 < st.dwell) {
+ st.dwell -= SEGMENT_SEC;
+ return;
+ } else st.dwell = 0;
+
// If the next move is not ready try to load it
if (!st.move_ready) {
_request_exec_move();
+ _end_move();
return;
}
- // Wait until all motors have energized
- if (motor_energizing()) return;
-
// Start move
if (st.move_type == MOVE_TYPE_LINE)
for (int motor = 0; motor < MOTORS; motor++)
motor_load_move(motor);
+ else _end_move();
+
if (st.move_type != MOVE_TYPE_NULL) {
- TIMER_STEP.PER = SEGMENT_PERIOD;
st.busy = true;
// Start dwell
}
+/// Step timer interrupt routine.
+ISR(STEP_TIMER_ISR) {
+ _load_move();
+}
+
+
/* Prepare the next move
*
* This function precomputes the next pulse segment (move) so it can
*/
stat_t st_prep_line(const float target[]) {
// Trap conditions that would prevent queueing the line
- if (st.move_ready) return ALARM(STAT_INTERNAL_ERROR);
+ ASSERT(!st.move_ready);
// Setup segment parameters
st.move_type = MOVE_TYPE_LINE;
// Prepare motor moves
- for (int motor = 0; motor < MOTORS; motor++)
- RITORNO(motor_prep_move(motor, round(target[motor])));
+ for (int motor = 0; motor < MOTORS; motor++) {
+ ASSERT(isfinite(target[motor]));
+ motor_prep_move(motor, round(target[motor]));
+ }
st.move_queued = true; // signal prep buffer ready (do this last)
/// Add a dwell to the move buffer
void st_prep_dwell(float seconds) {
- if (st.move_ready) ALARM(STAT_INTERNAL_ERROR);
+ ASSERT(!st.move_ready);
st.move_type = MOVE_TYPE_DWELL;
st.prep_dwell = seconds;
st.move_queued = true; // signal prep buffer ready
/// Fast inverse square root originally from Quake III Arena code. Original
/// comments left intact.
/// See: https://en.wikipedia.org/wiki/Fast_inverse_square_root
-float invsqrt(float number) {
- const float threehalfs = 1.5F;
-
- float x2 = number * 0.5F;
- float y = number;
- int32_t i = *(int32_t *)&y; // evil floating point bit level hacking
- i = 0x5f3759df - (i >> 1); // what the fuck?
- y = *(float *)&i;
- y = y * (threehalfs - x2 * y * y); // 1st iteration
- y = y * (threehalfs - x2 * y * y); // 2nd iteration, this can be removed
-
- return y;
+float invsqrt(float x) {
+ // evil floating point bit level hacking
+ union {
+ float f;
+ int32_t i;
+ } u;
+
+ const float xhalf = x * 0.5f;
+ u.f = x;
+ u.i = 0x5f3759df - (u.i >> 1); // what the fuck?
+ u.f = u.f * (1.5f - xhalf * u.f * u.f); // 1st iteration
+ u.f = u.f * (1.5f - xhalf * u.f * u.f); // 2nd iteration, can be removed
+
+ return u.f;
}
int32_t, char);
+// Eq functions
+#define EQ_FUNC(TYPE) \
+ inline static bool var_eq_##TYPE(const TYPE a, const TYPE b) {return a == b;}
+MAP(EQ_FUNC, SEMI, flags_t, string, pstring, uint8_t, uint16_t, int32_t, char);
+
+
// String
-static void var_print_string(string s) {
- printf_P(PSTR("\"%s\""), s);
-}
+static void var_print_string(string s) {printf_P(PSTR("\"%s\""), s);}
+
// Program string
-static void var_print_pstring(pstring s) {
- printf_P(PSTR("\"%"PRPSTR"\""), s);
-}
+static void var_print_pstring(pstring s) {printf_P(PSTR("\"%"PRPSTR"\""), s);}
// Flags
-extern void print_status_flags(uint8_t x);
-
static void var_print_flags_t(uint8_t x) {
+ extern void print_status_flags(uint8_t x);
print_status_flags(x);
}
// Float
+static bool var_eq_float(float a, float b) {
+ return a == b || (isnan(a) && isnan(b));
+}
+
+
static void var_print_float(float x) {
- char buf[20];
+ if (isnan(x)) printf_P(PSTR("\"nan\""));
+ else if (isinf(x)) printf_P(PSTR("\"%cinf\""), x < 0 ? '-' : '+');
+
+ else {
+ char buf[20];
- int len = sprintf_P(buf, PSTR("%.6f"), x);
+ int len = sprintf_P(buf, PSTR("%.6f"), x);
- // Remove trailing zeros
- for (int i = len; 0 < i; i--) {
- if (buf[i - 1] == '.') buf[i - 1] = 0;
- else if (buf[i - 1] == '0') {
- buf[i - 1] = 0;
- continue;
+ // Remove trailing zeros
+ for (int i = len; 0 < i; i--) {
+ if (buf[i - 1] == '.') buf[i - 1] = 0;
+ else if (buf[i - 1] == '0') {
+ buf[i - 1] = 0;
+ continue;
+ }
+
+ break;
}
- break;
+ printf(buf);
}
-
- printf(buf);
}
// Bool
-static void var_print_bool(bool x) {
- printf_P(x ? PSTR("true") : PSTR("false"));
-}
+inline static bool var_eq_bool(float a, float b) {return a == b;}
+static void var_print_bool(bool x) {printf_P(x ? PSTR("true") : PSTR("false"));}
static bool var_parse_bool(const char *value) {
#endif
// uint8
-static void var_print_uint8_t(uint8_t x) {
- printf_P(PSTR("%"PRIu8), x);
-}
+static void var_print_uint8_t(uint8_t x) {printf_P(PSTR("%"PRIu8), x);}
static uint8_t var_parse_uint8_t(const char *value) {
bool reported = false;
-#define VAR(NAME, CODE, TYPE, INDEX, ...) \
- IF(INDEX)(for (int i = 0; i < (INDEX ? INDEX : 1); i++)) { \
- TYPE value = get_##NAME(IF(INDEX)(i)); \
- TYPE last = (NAME##_state)IF(INDEX)([i]); \
- \
- if (value != last || full) { \
- (NAME##_state)IF(INDEX)([i]) = value; \
- \
- if (!reported) { \
- reported = true; \
- putchar('{'); \
- } else putchar(','); \
- \
- printf_P \
- (IF_ELSE(INDEX)(indexed_code_fmt, code_fmt), \
- IF(INDEX)(INDEX##_LABEL[i],) #CODE); \
- \
- var_print_##TYPE(value); \
- } \
+#define VAR(NAME, CODE, TYPE, INDEX, ...) \
+ IF(INDEX)(for (int i = 0; i < (INDEX ? INDEX : 1); i++)) { \
+ TYPE value = get_##NAME(IF(INDEX)(i)); \
+ TYPE last = (NAME##_state)IF(INDEX)([i]); \
+ \
+ if (!var_eq_##TYPE(value, last) || full) { \
+ (NAME##_state)IF(INDEX)([i]) = value; \
+ \
+ if (!reported) { \
+ reported = true; \
+ putchar('{'); \
+ } else putchar(','); \
+ \
+ printf_P \
+ (IF_ELSE(INDEX)(indexed_code_fmt, code_fmt), \
+ IF(INDEX)(INDEX##_LABEL[i],) #CODE); \
+ \
+ var_print_##TYPE(value); \
+ } \
}
#include "vars.def"
VAR(status_flags, mf, uint8_t, MOTORS, 0, "Motor status flags")
VAR(status_strings, ms, flags_t, MOTORS, 0, "Motor status strings")
VAR(encoder, en, int32_t, MOTORS, 0, "Motor encoder")
+VAR(error, ee, int32_t, MOTORS, 0, "Motor position error")
VAR(motor_fault, fa, bool, 0, 0, "Motor fault status")
{
"name": "bbctrl",
- "version": "0.1.8",
+ "version": "0.1.9",
"homepage": "https://github.com/buildbotics/rpi-firmware",
"license": "GPL 3+",
projects / bbctrl-firmware / commitdiff