From: Joseph Coffland Date: Sat, 12 Mar 2016 07:52:25 +0000 (-0800) Subject: Fixed timing issues X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=2452b63b09ec93cd389e700475fd10c28756b38c;p=bbctrl-firmware Fixed timing issues --- diff --git a/src/config.h b/src/config.h index 5d793e9..8d10b1a 100644 --- a/src/config.h +++ b/src/config.h @@ -117,8 +117,8 @@ // Axis settings -#define VELOCITY_MAX 1000 -#define FEEDRATE_MAX 4000 +#define VELOCITY_MAX 575 +#define FEEDRATE_MAX VELOCITY_MAX // see canonical_machine.h cmAxisMode for valid values #define X_AXIS_MODE AXIS_STANDARD diff --git a/src/encoder.c b/src/encoder.c index 4d2f09a..eca8b59 100644 --- a/src/encoder.c +++ b/src/encoder.c @@ -37,34 +37,32 @@ #include -enEncoders_t en; +enEncoder_t en[MOTORS]; void encoder_init() { - memset(&en, 0, sizeof(en)); // clear all values, pointers and status + memset(en, 0, sizeof(en)); // clear all values, pointers and status } -/* - * Set encoder values to a current step count +/* Set encoder values to a current step count * - * Sets the encoder_position steps. Takes floating point steps as input, + * Sets encoder_steps. Takes floating point steps as input, * writes integer steps. So it's not an exact representation of machine * position except if the machine is at zero. */ void en_set_encoder_steps(uint8_t motor, float steps) { - en.en[motor].encoder_steps = (int32_t)round(steps); + en[motor].encoder_steps = (int32_t)round(steps); } -/* - * The stepper ISR count steps into steps_run(). These values are - * accumulated to encoder_position during LOAD (HI interrupt - * level). The encoder position is therefore always stable. But be - * advised: the position lags target and position valaues - * elsewherein the system becuase the sample is taken when the +/* The stepper ISR counts steps into steps_run. These values are + * accumulated to encoder_steps during LOAD (HI interrupt + * level). The encoder_steps is therefore always stable. But be + * advised: the position lags target and position values + * elsewhere in the system becuase the sample is taken when the * steps for that segment are complete. */ float en_read_encoder(uint8_t motor) { - return (float)en.en[motor].encoder_steps; + return en[motor].encoder_steps; } diff --git a/src/encoder.h b/src/encoder.h index a40dff6..2e7800c 100644 --- a/src/encoder.h +++ b/src/encoder.h @@ -112,12 +112,11 @@ #include -// used to abstract the encoder code out of the stepper so it can be managed in -// one place -#define SET_ENCODER_STEP_SIGN(m, s) en.en[m].step_sign = s; -#define INCREMENT_ENCODER(m) en.en[m].steps_run += en.en[m].step_sign; +// macros used in stepper.c +#define SET_ENCODER_STEP_SIGN(m, s) en[m].step_sign = s; +#define INCREMENT_ENCODER(m) en[m].steps_run += en[m].step_sign; #define ACCUMULATE_ENCODER(m) \ - en.en[m].encoder_steps += en.en[m].steps_run; en.en[m].steps_run = 0; + do {en[m].encoder_steps += en[m].steps_run; en[m].steps_run = 0;} while (0) typedef struct enEncoder { // one real or virtual encoder per controlled motor @@ -127,11 +126,7 @@ typedef struct enEncoder { // one real or virtual encoder per controlled motor } enEncoder_t; -typedef struct enEncoders { - enEncoder_t en[MOTORS]; // runtime encoder structures -} enEncoders_t; - -extern enEncoders_t en; +extern enEncoder_t en[MOTORS]; void encoder_init(); diff --git a/src/hardware.h b/src/hardware.h index e330288..c81bddd 100644 --- a/src/hardware.h +++ b/src/hardware.h @@ -151,19 +151,25 @@ enum cfgPortBits { // motor control port bit positions #define TIMER_DDA TCC0 // DDA timer (see stepper.h) #define TIMER_TMC2660 TCC1 // TMC2660 timer (see tmc2660.h) #define TIMER_PWM1 TCD1 // PWM timer #1 (see pwm.c) -#define TIMER_PWM2 TCE1 // PWM timer #2 (see pwm.c) -#define TIMER_MOTOR1 TCD0 -#define TIMER_MOTOR2 TCE0 -#define TIMER_MOTOR3 TCF0 -#define TIMER_MOTOR4 TCF1 +#define TIMER_PWM2 TCD1 // PWM timer #2 (see pwm.c) +#define TIMER_MOTOR1 TCE1 +#define TIMER_MOTOR2 TCF0 +#define TIMER_MOTOR3 TCE0 +#define TIMER_MOTOR4 TCD0 // Timer setup for stepper and dwells -#define FREQUENCY_DDA 50000 // DDA frequency in hz. -#define STEP_TIMER_DISABLE 0 // turn timer off -#define STEP_TIMER_ENABLE 1 // turn timer clock on -#define STEP_TIMER_WGMODE 0 // normal mode (count to TOP and rollover) +#define FREQUENCY_DDA 25000 // DDA frequency in hz. +#define STEP_TIMER_DISABLE 0 // turn timer off +#define STEP_TIMER_ENABLE 1 // turn timer clock on +#define STEP_TIMER_WGMODE 0 // normal mode (count to TOP and rollover) #define TIMER_DDA_ISR_vect TCC0_OVF_vect -#define TIMER_DDA_INTLVL 3 // Timer overflow HI +#define TIMER_DDA_INTLVL 3 // Timer overflow HI + +#define PWM1_CTRLB (3 | TC1_CCBEN_bm) // single slope PWM channel B +#define PWM1_ISR_vect TCD1_CCB_vect +#define PWM2_CTRLA_CLKSEL TC_CLKSEL_DIV1_gc +#define PWM2_CTRLB 3 // single slope PWM no output +#define PWM2_ISR_vect TCE1_CCB_vect /* Device singleton - global structure to allow iteration through similar devices diff --git a/src/pwm.c b/src/pwm.c index 5331d3c..3a70e53 100644 --- a/src/pwm.c +++ b/src/pwm.c @@ -59,23 +59,10 @@ pwmSingleton_t pwm; * TC_CLKSEL_DIV8_gc - good for about 62 Hz to 16 KHz * TC_CLKSEL_DIV64_gc - good for about 8 Hz to 2 Khz */ -#define PWM1_CTRLA_CLKSEL TC_CLKSEL_DIV1_gc // starting clock select value -/// single slope PWM enabled on channel B -#define PWM1_CTRLB (3 | TC0_CCBEN_bm) -#define PWM1_ISR_vect TCD1_CCB_vect -/// timer interrupt level (0=off, 1=lo, 2=med, 3=hi) -#define PWM1_INTCTRLB 0 +#define PWM1_CTRLA_CLKSEL TC_CLKSEL_DIV1_gc // starting clock select value -#define PWM2_CTRLA_CLKSEL TC_CLKSEL_DIV1_gc -/// single slope PWM enabled, no output channel -#define PWM2_CTRLB 3 -#define PWM2_ISR_vect TCE1_CCB_vect -/// timer interrupt level (0=off, 1=lo, 2=med, 3=hi) -#define PWM2_INTCTRLB 0 - -/* - * Initialize pwm channels +/* Initialize pwm channels * * Notes: * - Whatever level interrupts you use must be enabled in main() @@ -83,6 +70,8 @@ pwmSingleton_t pwm; * (stepper.c) */ void pwm_init() { + return; // Don't init PWM for now. TODO fix this + gpio_set_bit_off(SPINDLE_PWM); // setup PWM channel 1 @@ -94,7 +83,6 @@ void pwm_init() { // initialize starting clock operating range pwm.p[PWM_1].ctrla = PWM1_CTRLA_CLKSEL; pwm.p[PWM_1].timer->CTRLB = PWM1_CTRLB; - pwm.p[PWM_1].timer->INTCTRLB = PWM1_INTCTRLB; // set interrupt level // setup PWM channel 2 // clear all values, pointers and status @@ -103,7 +91,6 @@ void pwm_init() { pwm.p[PWM_2].timer = &TIMER_PWM2; pwm.p[PWM_2].ctrla = PWM2_CTRLA_CLKSEL; pwm.p[PWM_2].timer->CTRLB = PWM2_CTRLB; - pwm.p[PWM_2].timer->INTCTRLB = PWM2_INTCTRLB; pwm.c[PWM_1].frequency = P1_PWM_FREQUENCY; pwm.c[PWM_1].cw_speed_lo = P1_CW_SPEED_LO; @@ -118,20 +105,13 @@ void pwm_init() { } -ISR(PWM1_ISR_vect) {} - - -ISR(PWM2_ISR_vect) {} - - -/* - * Set PWM channel frequency +/* Set PWM channel frequency * - * @param channel PWM channel - * @param freq PWM frequency in Khz as a float + * @param channel PWM channel + * @param freq PWM frequency in Khz as a float * - * Assumes 32MHz clock. - * Doesn't turn time on until duty cycle is set + * Assumes 32MHz clock. + * Doesn't turn time on until duty cycle is set */ stat_t pwm_set_freq(uint8_t chan, float freq) { if (chan > PWMS) return STAT_NO_SUCH_DEVICE; @@ -169,14 +149,14 @@ stat_t pwm_set_freq(uint8_t chan, float freq) { /* * Set PWM channel duty cycle * - * @param channel PWM channel - * @param duty PWM duty cycle from 0% to 100% + * @param channel PWM channel + * @param duty PWM duty cycle from 0% to 100% * - * Setting duty cycle to 0 disables the PWM channel with output low - * Setting duty cycle to 100 disables the PWM channel with output high - * Setting duty cycle between 0 and 100 enables PWM channel + * Setting duty cycle to 0 disables the PWM channel with output low + * Setting duty cycle to 100 disables the PWM channel with output high + * Setting duty cycle between 0 and 100 enables PWM channel * - * The frequency must have been set previously + * The frequency must have been set previously */ stat_t pwm_set_duty(uint8_t chan, float duty) { if (duty < 0.0) return STAT_INPUT_LESS_THAN_MIN_VALUE; diff --git a/src/stepper.c b/src/stepper.c index 9ecdbaa..d7344d7 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -59,6 +59,7 @@ static stRunSingleton_t st_run; static void _load_move(); static void _request_load_move(); +static void _update_steps_per_unit(int motor); #define _f_to_period(f) (uint16_t)((float)F_CPU / (float)f) @@ -147,20 +148,14 @@ void stepper_init() { #endif // Init steps per unit - for (int m = 0; m < MOTORS; m++) - st_cfg.mot[m].steps_per_unit = - (360 * st_cfg.mot[m].microsteps) / - (st_cfg.mot[m].travel_rev * st_cfg.mot[m].step_angle); + for (int m = 0; m < MOTORS; m++) _update_steps_per_unit(m); st_reset(); // reset steppers to known state } -/// return true if runtime is busy: -/// Busy conditions: 1. motors are running, 2. dwell is running -uint8_t st_runtime_isbusy() { - return st_run.dda_ticks_downcount != 0; -} +/// Return true if motors or dwell are running +uint8_t st_runtime_isbusy() {return st_run.dda_ticks_downcount;} /// Reset stepper internals @@ -300,8 +295,8 @@ void st_request_exec_move() { /// Interrupt handler for calling exec function -/// Use ADC channel 0 as software interrupt ISR(ADCB_CH0_vect) { + // Use ADC channel 0 as software interrupt // Exec move if (st_pre.buffer_state == PREP_BUFFER_OWNED_BY_EXEC && mp_exec_move() != STAT_NOOP) { @@ -323,12 +318,11 @@ static void _request_load_move() { /// Interrupt handler for running the loader -/// Use ADC channel 1 as software interrupt ISR(ADCB_CH1_vect) { + // Use ADC channel 1 as software interrupt. // _load_move() can only be called be called from an ISR at the same or - // higher level as the DDA or dwell ISR. A software interrupt has been - // provided to allow a non-ISR to request a load - // (see st_request_load_move()) + // higher level as the DDA ISR. A software interrupt is used to allow a + // non-ISR to request a load. See st_request_load_move() _load_move(); } @@ -338,7 +332,7 @@ static inline void _load_motor_move(int motor) { stPrepMotor_t *prep_mot = &st_pre.mot[motor]; cfgMotor_t *cfg_mot = &st_cfg.mot[motor]; - // Set or zero runtime substep increment + // Set or zero, runtime substep increment run_mot->substep_increment = prep_mot->substep_increment; if (run_mot->substep_increment) { @@ -369,18 +363,13 @@ static inline void _load_motor_move(int motor) { } SET_ENCODER_STEP_SIGN(motor, prep_mot->step_sign); + } - // Enable the stepper and start motor power management - if (cfg_mot->power_mode != MOTOR_DISABLED) { - vports[motor]->OUT &= ~MOTOR_ENABLE_BIT_bm; // energize motor - run_mot->power_state = MOTOR_POWER_TIMEOUT_START; // set power management - } - - } else if (cfg_mot->power_mode == MOTOR_POWERED_IN_CYCLE) { - // Motor has 0 steps; might need to energize motor for power mode - // processing - vports[motor]->OUT &= ~MOTOR_ENABLE_BIT_bm; // energize motor - run_mot->power_state = MOTOR_POWER_TIMEOUT_START; + // Enable the stepper and start motor power management + if ((run_mot->substep_increment && cfg_mot->power_mode != MOTOR_DISABLED) || + cfg_mot->power_mode == MOTOR_POWERED_IN_CYCLE) { + vports[motor]->OUT &= ~MOTOR_ENABLE_BIT_bm; // energize motor + run_mot->power_state = MOTOR_POWER_TIMEOUT_START; // start power management } // Accumulate counted steps to the step position and zero out counted steps @@ -418,19 +407,13 @@ static void _load_move() { st_run.dda_ticks_X_substeps = st_pre.dda_ticks_X_substeps; _load_motor_move(MOTOR_1); -#if 1 < MOTORS _load_motor_move(MOTOR_2); -#endif -#if 2 < MOTORS _load_motor_move(MOTOR_3); -#endif -#if 3 < MOTORS _load_motor_move(MOTOR_4); -#endif // do this last TIMER_DDA.PER = st_pre.dda_period; - TIMER_DDA.CTRLA = STEP_TIMER_ENABLE; // enable the DDA timer + TIMER_DDA.CTRLA = STEP_TIMER_ENABLE; // enable the DDA timer } else if (st_pre.move_type == MOVE_TYPE_DWELL) { // handle dwells @@ -474,6 +457,7 @@ static void _load_move() { * NOTE: Many of the expressions are sensitive to casting and execution order to * avoid long-term accuracy errors due to floating point round off. One earlier * failed attempt was: + * * dda_ticks_X_substeps = * (int32_t)((microseconds / 1000000) * f_dda * dda_substeps); */ @@ -522,7 +506,7 @@ stat_t st_prep_line(float travel_steps[], float following_error[], // and flag. Putting this here computes the correct factor even if the motor // was dormant for some number of previous moves. Correction is computed // based on the last segment time actually used. - if (fabs(segment_time - st_pre.mot[motor].prev_segment_time) > 0.0000001) { + if (0.0000001 < fabs(segment_time - st_pre.mot[motor].prev_segment_time)) { // special case to skip first move if (fp_NOT_ZERO(st_pre.mot[motor].prev_segment_time)) { st_pre.mot[motor].accumulator_correction_flag = true; @@ -538,13 +522,13 @@ stat_t st_prep_line(float travel_steps[], float following_error[], // 'Nudge' correction strategy. Inject a single, scaled correction value // then hold off - if ((--st_pre.mot[motor].correction_holdoff < 0) && - (fabs(following_error[motor]) > STEP_CORRECTION_THRESHOLD)) { + if (--st_pre.mot[motor].correction_holdoff < 0 && + fabs(following_error[motor]) > STEP_CORRECTION_THRESHOLD) { st_pre.mot[motor].correction_holdoff = STEP_CORRECTION_HOLDOFF; correction_steps = following_error[motor] * STEP_CORRECTION_FACTOR; - if (correction_steps > 0) + if (0 < correction_steps) correction_steps = min3(correction_steps, fabs(travel_steps[motor]), STEP_CORRECTION_MAX); else correction_steps = max3(correction_steps, -fabs(travel_steps[motor]), @@ -555,7 +539,7 @@ stat_t st_prep_line(float travel_steps[], float following_error[], } #endif - // Compute substeb increment. The accumulator must be *exactly* the + // Compute substep increment. The accumulator must be *exactly* the // incoming fractional steps times the substep multiplier or positional // drift will occur. Rounding is performed to eliminate a negative bias // in the uint32 conversion that results in long-term negative drift. @@ -590,7 +574,7 @@ void st_prep_command(void *bf) { void st_prep_dwell(float microseconds) { st_pre.move_type = MOVE_TYPE_DWELL; st_pre.dda_period = _f_to_period(FREQUENCY_DDA); - st_pre.dda_ticks = (uint32_t)(microseconds / 1000000 * FREQUENCY_DDA); + st_pre.dda_ticks = microseconds / 1000000 * FREQUENCY_DDA; st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal prep buffer ready } @@ -603,22 +587,22 @@ uint16_t get_mvel(int index) {return 0;} uint16_t get_mjerk(int index) {return 0;} -void update_steps_per_unit(int index) { - st_cfg.mot[index].steps_per_unit = - (360 * st_cfg.mot[index].microsteps) / - (st_cfg.mot[index].travel_rev * st_cfg.mot[index].step_angle); +static void _update_steps_per_unit(int motor) { + st_cfg.mot[motor].steps_per_unit = + (360 * st_cfg.mot[motor].microsteps) / + (st_cfg.mot[motor].travel_rev * st_cfg.mot[motor].step_angle); } void set_ang(int index, float value) { st_cfg.mot[index].step_angle = value; - update_steps_per_unit(index); + _update_steps_per_unit(index); } void set_trvl(int index, float value) { st_cfg.mot[index].travel_rev = value; - update_steps_per_unit(index); + _update_steps_per_unit(index); } @@ -630,7 +614,7 @@ void set_mstep(int index, uint16_t value) { } st_cfg.mot[index].microsteps = value; - update_steps_per_unit(index); + _update_steps_per_unit(index); }