From: Joseph Coffland Date: Sat, 8 Apr 2017 07:58:27 +0000 (-0700) Subject: Back to controlled DMA counting X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=a210c55643136f9a94b8975369bab90cce68921c;p=bbctrl-firmware Back to controlled DMA counting --- diff --git a/avr/MoveLifecycleCalls.md b/avr/MoveLifecycleCalls.md index 2fca19b..b982100 100644 --- a/avr/MoveLifecycleCalls.md +++ b/avr/MoveLifecycleCalls.md @@ -1,24 +1,25 @@ # 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() @@ -34,10 +35,13 @@ # 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 diff --git a/avr/src/axis.c b/avr/src/axis.c index 3bfa5ed..50ee734 100644 --- a/avr/src/axis.c +++ b/avr/src/axis.c @@ -80,7 +80,7 @@ int axis_get_motor(int axis) {return motor_map[axis];} 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 } diff --git a/avr/src/config.h b/avr/src/config.h index d2db1bd..eccc5d7 100644 --- a/avr/src/config.h +++ b/avr/src/config.h @@ -141,11 +141,6 @@ enum { #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 @@ -161,12 +156,13 @@ enum { #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 @@ -220,8 +216,7 @@ enum { #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 diff --git a/avr/src/main.c b/avr/src/main.c index 4c79e69..9443960 100644 --- a/avr/src/main.c +++ b/avr/src/main.c @@ -87,7 +87,7 @@ int main() { 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 diff --git a/avr/src/motor.c b/avr/src/motor.c index e46aef3..9916eaf 100644 --- a/avr/src/motor.c +++ b/avr/src/motor.c @@ -42,6 +42,8 @@ #include "plan/runtime.h" #include "plan/calibrate.h" +#include + #include #include #include @@ -51,7 +53,6 @@ typedef enum { MOTOR_OFF, MOTOR_IDLE, // motor stopped and may be partially energized - MOTOR_ENERGIZING, MOTOR_ACTIVE } motor_power_state_t; @@ -70,19 +71,25 @@ typedef struct { 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; @@ -122,6 +129,13 @@ static motor_t motors[MOTORS] = { 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; @@ -135,11 +149,12 @@ void motor_init() { 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; @@ -157,8 +172,9 @@ 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 = 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); @@ -199,24 +215,7 @@ float motor_get_stall_homing_velocity(int motor) { } -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;} @@ -228,6 +227,7 @@ void motor_set_microsteps(int motor, uint16_t microsteps) { } motors[motor].microsteps = microsteps; + _update_config(motor); drv8711_set_microsteps(motor, microsteps); } @@ -235,11 +235,16 @@ void motor_set_microsteps(int motor, uint16_t 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 @@ -276,12 +281,10 @@ static void _update_power(int motor) { // 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; @@ -295,15 +298,6 @@ static void _update_power(int motor) { } -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]; @@ -345,84 +339,129 @@ void motor_error_callback(int motor, motor_flags_t errors) { } -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: @@ -435,32 +474,29 @@ stat_t motor_prep_move(int motor, int32_t target) { _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;} @@ -484,10 +520,20 @@ void set_motor_axis(int motor, uint8_t value) { 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;} @@ -545,7 +591,8 @@ void print_status_flags(uint8_t flags) { 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 diff --git a/avr/src/motor.h b/avr/src/motor.h index 2bf71c1..54cb7c4 100644 --- a/avr/src/motor.h +++ b/avr/src/motor.h @@ -69,7 +69,6 @@ int motor_get_axis(int motor); 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); @@ -79,11 +78,10 @@ bool motor_error(int motor); 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); diff --git a/avr/src/plan/exec.c b/avr/src/plan/exec.c index 17ea41a..136dc96 100644 --- a/avr/src/plan/exec.c +++ b/avr/src/plan/exec.c @@ -122,6 +122,9 @@ static stat_t _exec_aline_section(float length, float Vi, float Vt) { 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; } diff --git a/avr/src/stepper.c b/avr/src/stepper.c index 6181043..d176d0b 100644 --- a/avr/src/stepper.c +++ b/avr/src/stepper.c @@ -71,7 +71,7 @@ void stepper_init() { // 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 } @@ -91,7 +91,7 @@ bool st_is_busy() {return st.busy;} /// 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(); @@ -120,45 +120,48 @@ static void _request_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 @@ -176,6 +179,12 @@ ISR(STEP_TIMER_ISR) { } +/// 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 @@ -190,14 +199,16 @@ ISR(STEP_TIMER_ISR) { */ 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) @@ -207,7 +218,7 @@ stat_t st_prep_line(const float target[]) { /// 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 diff --git a/avr/src/util.c b/avr/src/util.c index 021c1a1..1a76542 100644 --- a/avr/src/util.c +++ b/avr/src/util.c @@ -34,16 +34,18 @@ /// 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; } diff --git a/avr/src/vars.c b/avr/src/vars.c index 6b097a3..f942937 100644 --- a/avr/src/vars.c +++ b/avr/src/vars.c @@ -60,43 +60,55 @@ MAP(TYPE_NAME, SEMI, flags_t, string, pstring, float, uint8_t, uint16_t, 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); } @@ -106,9 +118,8 @@ static float var_parse_float(const char *value) { // 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) { @@ -134,9 +145,7 @@ static int8_t var_parse_int8_t(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) { @@ -211,25 +220,25 @@ void vars_report(bool full) { 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" diff --git a/avr/src/vars.def b/avr/src/vars.def index 53763d5..9d63e3b 100644 --- a/avr/src/vars.def +++ b/avr/src/vars.def @@ -45,6 +45,7 @@ VAR(active_current, ac, float, MOTORS, 0, "Motor current now") 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") diff --git a/package.json b/package.json index f4f4734..c85da92 100644 --- a/package.json +++ b/package.json @@ -1,6 +1,6 @@ { "name": "bbctrl", - "version": "0.1.8", + "version": "0.1.9", "homepage": "https://github.com/buildbotics/rpi-firmware", "license": "GPL 3+",