SpaceForOne
Published © GPL3+

Arduino Based Embroidery Machine

Convert a plain sewing machine into an embroidery machine to personalize everything made from fabric.

AdvancedShowcase (no instructions)Over 4 days9,279
Arduino Based Embroidery Machine

Things used in this project

Story

Read more

Custom parts and enclosures

3D Files for Embroidery Machine

Embroidery File Tux

The SVG is from https://lowtechlinux.com/2023/04/05/new-digitized-tux-w-v-2-2-0/. I dajusted some paths and edited the gcode such that it would work with my machine. The order of the colors is gray - yellow - black.

Code

motor.c

C/C++
Modified motor.c file. The variable startpoint_steps seems to experience a memory collision in the BSS segment so it will not be put there for the ATMega328/168
#include    "motor.h"

/** \file
    \brief Digital differential analyser - this is where we figure out which steppers need to move, and when they need to move
*/

#include    <string.h>
#include    <stdlib.h>
#include    <math.h>

#include    "maths.h"
#include    "kinematics.h"
#include    "timer.h"
#include    "serial.h"
#include    "queue.h"

#include    "pinio.h"
#include    "sensors_control.h"

/*
    position tracking
*/

/// \var startpoint
/// \brief target position of last move in queue
TARGET BSS startpoint;

/// \var startpoint_steps
/// \brief target position of last move in queue, expressed in steps
#if defined (__AVR_ATmega168__) || defined (__AVR_ATmega328__) || \
    defined (__AVR_ATmega328P__)
TARGET startpoint_steps;
#else
TARGET BSS startpoint_steps;
#endif

/// \var steps_per_m_P
/// \brief motor steps required to advance one meter on each axis
static const axes_uint32_t PROGMEM steps_per_m_P = {
  STEPS_PER_M_X,
  STEPS_PER_M_Y
  #ifdef STEPS_PER_M_Z
    ,STEPS_PER_M_Z
  #endif
  #ifdef STEPS_PER_M_E
    ,STEPS_PER_M_E
  #endif
};

/// \var maximum_feedrate_P
/// \brief maximum allowed feedrate on each axis
static const axes_uint32_t PROGMEM maximum_feedrate_P = {
  MAXIMUM_FEEDRATE_X,
  MAXIMUM_FEEDRATE_Y
  #ifdef MAXIMUM_FEEDRATE_Z
    ,MAXIMUM_FEEDRATE_Z
  #endif
  #ifdef MAXIMUM_FEEDRATE_E
    ,MAXIMUM_FEEDRATE_E
  #endif
};

/// \var current_position
/// \brief actual position of carriage
/// \todo make current_position = real_position (from endstops) + offset from G28 and friends
TARGET BSS current_position;

/// \var move_state
/// \brief numbers for tracking the current state of movement
MOVE_STATE BSS move_state;

/// \var c0_P
/// \brief Initialization constant for the ramping algorithm. Timer cycles for
///        first step interval.
static const axes_uint32_t PROGMEM c0_P = {
  (uint32_t)((double)F_CPU / SQRT((double)STEPS_PER_M_X * ACCELERATION / 2000.)),
  (uint32_t)((double)F_CPU / SQRT((double)STEPS_PER_M_Y * ACCELERATION / 2000.))
  #ifdef STEPS_PER_M_Z
  ,(uint32_t)((double)F_CPU / SQRT((double)STEPS_PER_M_Z * ACCELERATION / 2000.))
  #endif
  #ifdef STEPS_PER_M_E
  ,(uint32_t)((double)F_CPU / SQRT((double)STEPS_PER_M_E * ACCELERATION / 2000.))
  #endif
};

/*! Inititalise DDA movement structures
*/
void dda_init(void) {
    // set up default feedrate
    if (startpoint.F == 0)
        startpoint.F = SEARCH_FEEDRATE_Y;
  #ifdef STEPS_PER_M_E
    if (startpoint.e_multiplier == 0)
    startpoint.e_multiplier =  256;
#endif
  if (startpoint.f_multiplier == 0)
    startpoint.f_multiplier = 256;
}

/*! Distribute a new startpoint to DDA's internal structures without any movement.

    This is needed for example after homing or a G92. The new location must be in startpoint already.
*/
void dda_new_startpoint(void) {
    axes_um_to_steps(startpoint.axis, startpoint_steps.axis);
}

/*! Set the direction of the 'n' axis
*/
static void set_direction(DDA *dda, uint8_t n, int32_t delta) {
  uint8_t dir = (delta >= 0) ? 1 : 0;

  if (n == X)
    dda->x_direction = dir;
  else if (n == Y)
    dda->y_direction = dir;
#ifdef STEPS_PER_M_Z
  else if (n == Z)
    dda->z_direction = dir;
#endif
#ifdef STEPS_PER_M_E
  else if (n == E)
    dda->e_direction = dir;
#endif
}

/*! Find the direction of the 'n' axis
*/
static int8_t get_direction(DDA *dda, uint8_t n) {
  if ((n == X && dda->x_direction) ||
      (n == Y && dda->y_direction)
#ifdef STEPS_PER_M_Z
      || (n == Z && dda->z_direction)
#endif
#ifdef STEPS_PER_M_E
      || (n == E && dda->e_direction)
#endif
    )
    return 1;
  else
    return -1;
}

void dda_create(DDA *dda, const TARGET *target) {
  axes_uint32_t delta_um;
  axes_int32_t steps;
    uint32_t    distance, c_limit, c_limit_calc;
  uint8_t i;
  static DDA* prev_dda = NULL;
  #ifdef LOOKAHEAD
  // Number the moves to identify them; allowed to overflow.
  static uint8_t idcnt = 0;

  if (prev_dda && prev_dda->done)
    prev_dda = NULL;
  #endif

  // We end at the passed target.
  memcpy(&(dda->endpoint), target, sizeof(TARGET));
#ifdef STEPS_PER_M_Z
    if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
        sersendf_P(PSTR("\nCreate: X %lq  Y %lq  Z %lq  F %lu\n"),
               dda->endpoint.axis[X], dda->endpoint.axis[Y],
               dda->endpoint.axis[Z], dda->endpoint.F );
#else
    sersendf_P(PSTR("\nCreate: X %lq  Y %lq  F %lu\n"),
               dda->endpoint.axis[X], dda->endpoint.axis[Y], dda->endpoint.F );
#endif


  // Apply feedrate multiplier.
  if (dda->endpoint.f_multiplier != 256) {
    dda->endpoint.F *= dda->endpoint.f_multiplier;
    dda->endpoint.F += 128;
    dda->endpoint.F /= 256;
  }

  #ifdef LOOKAHEAD
    // Set the start and stop speeds to zero for now = full stops between
    // moves. Also fallback if lookahead calculations fail to finish in time.
    dda->crossF = 0;
    dda->start_steps = 0;
    dda->end_steps = 0;
    // Give this move an identifier.
    dda->id = idcnt++;
  #endif

  // Handle bot axes. They're subject to kinematics considerations.
  code_axes_to_stepper_axes(&startpoint, target, delta_um, steps);
#ifdef STEPS_PER_M_Z
  for (i = X; i <= Z; i++) {
#else
  for (i = X; i <= Y; i++) {
#endif
    int32_t delta_steps;

    delta_steps = steps[i] - startpoint_steps.axis[i];
    dda->delta[i] = (uint32_t)labs(delta_steps);
    startpoint_steps.axis[i] = steps[i];

    set_direction(dda, i, delta_steps);
    #ifdef LOOKAHEAD
      // Also displacements in micrometers, but for the lookahead alogrithms.
      // TODO: this is redundant. delta_um[] and dda->delta_um[] differ by
      //       just signedness and storage location. Ideally, dda is used
      //       as storage place only if neccessary (LOOKAHEAD turned on?)
      //       because this space is multiplied by the movement queue size.
      //
      // Update 2014/10: it was tried to use delta_um[]'s sign to set stepper
      //                 direction in dda_start() to allow getting rid of
      //                 some of this redundancy, but this increases dda_start()
      //                 by at least 20 clock cycles. Not good for performance.
      //                 Tried code can be found in the archive folder.
      dda->delta_um[i] = (delta_steps >= 0) ?
                         (int32_t)delta_um[i] : -(int32_t)delta_um[i];
    #endif
  }

#ifdef STEPS_PER_M_E
  // Handle extruder axes. They act independently from the bots kinematics
  // type, but are subject to other special handling.
  steps[E] = um_to_steps(target->axis[E], E);

  // Apply extrusion multiplier.
  if (target->e_multiplier != 256) {
  steps[E] *= target->e_multiplier;
    steps[E] += 128;
    steps[E] /= 256;
  }

  if ( ! target->e_relative) {
    int32_t delta_steps;

    delta_um[E] = (uint32_t)labs(target->axis[E] - startpoint.axis[E]);
    delta_steps = steps[E] - startpoint_steps.axis[E];
    dda->delta[E] = (uint32_t)labs(delta_steps);
    startpoint_steps.axis[E] = steps[E];

    set_direction(dda, E, delta_steps);
    #ifdef LOOKAHEAD
      // Also displacements in micrometers, but for the lookahead alogrithms.
      // TODO: this is redundant. delta_um[] and dda->delta_um[] differ by
      //       just signedness and storage location. Ideally, dda is used
      //       as storage place only if neccessary (LOOKAHEAD turned on?)
      //       because this space is multiplied by the movement queue size.
      dda->delta_um[E] = (delta_steps >= 0) ?
                         (int32_t)delta_um[E] : -(int32_t)delta_um[E];
    #endif
  }
  else {
    // When we get more extruder axes:
    // for (i = E; i < AXIS_COUNT; i++) { ...
    delta_um[E] = (uint32_t)labs(target->axis[E]);
    dda->delta[E] = (uint32_t)labs(steps[E]);
    #ifdef LOOKAHEAD
      dda->delta_um[E] = target->axis[E];
    #endif
    dda->e_direction = (target->axis[E] >= 0)?1:0;
    }
#endif
    if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
    sersendf_P(PSTR("[%ld,%ld,%ld,%ld]"),
               target->axis[X] - startpoint.axis[X], target->axis[Y] - startpoint.axis[Y],
             #ifdef STEPS_PER_M_Z
               target->axis[Z] - startpoint.axis[Z],
             #else
               0,
             #endif
             #ifdef STEPS_PER_M_E
               target->axis[E] - startpoint.axis[E]
             #else
               0
             #endif
              );

  // Admittedly, this looks like it's overcomplicated. Why store three 32-bit
  // values if storing an axis number would be fully sufficient? Well, I'm not
  // sure, but my feeling says that when we achieve true circles and Beziers,
  // we'll have total_steps which matches neither of X, Y, Z or E. Accordingly,
  // keep it for now. --Traumflug
  for (i = X; i < AXIS_COUNT; i++) {
    if (i == X || dda->delta[i] > dda->total_steps) {
      dda->fast_axis = i;
      dda->total_steps = dda->delta[i];
      dda->fast_um = delta_um[i];
      dda->fast_spm = pgm_read_dword(&steps_per_m_P[i]);
    }
  }

    if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
        sersendf_P(PSTR(" [ts:%lu"), dda->total_steps);

    // null moves should be accepted anyway
    //if (dda->total_steps == 0) {
    //    dda->nullmove = 1;
    //}
    //else
    {
        // get steppers ready to go
        //power_on();
        stepper_enable();
        x_enable();
        y_enable();
#ifdef STEPS_PER_M_Z
        z_enable();
#endif
#ifdef STEPS_PER_M_E
        e_enable();
#endif
#ifdef STEPS_PER_M_Z
        // since it's unusual to combine X, Y and Z changes in a single move on reprap, check if we can use simpler approximations before trying the full 3d approximation.
        if (delta_um[Z] == 0)
            distance = approx_distance(delta_um[X], delta_um[Y]);
        else if (delta_um[X] == 0 && delta_um[Y] == 0)
            distance = delta_um[Z];
        else
            distance = approx_distance_3(delta_um[X], delta_um[Y], delta_um[Z]);
    #ifdef STEPS_PER_M_E
        if (distance < 2)
            distance = delta_um[E];
    #endif
#else
        distance = approx_distance(delta_um[X], delta_um[Y]);
#endif
        if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
            sersendf_P(PSTR(",ds:%lu"), distance);


            // pre-calculate move speed in millimeter microseconds per step minute for less math in interrupt context
            // mm (distance) * 60000000 us/min / step (total_steps) = mm.us per step.min
            //   note: um (distance) * 60000 == mm * 60000000
            // so in the interrupt we must simply calculate
            // mm.us per step.min / mm per min (F) = us per step

            // break this calculation up a bit and lose some precision because 300,000um * 60000 is too big for a uint32
            // calculate this with a uint64 if you need the precision, but it'll take longer so routines with lots of short moves may suffer
            // 2^32/6000 is about 715mm which should be plenty

            // changed * 10 to * (F_CPU / 100000) so we can work in cpu_ticks rather than microseconds.
            // timer.c timer_set() routine altered for same reason

            // changed distance * 6000 .. * F_CPU / 100000 to
            //         distance * 2400 .. * F_CPU / 40000 so we can move a distance of up to 1800mm without overflowing
            uint32_t move_duration = ((distance * 2400) / dda->total_steps) * (F_CPU / 40000);

        // similarly, find out how fast we can run our axes.
        // do this for each axis individually, as the combined speed of two or more axes can be higher than the capabilities of a single one.
    // TODO: instead of calculating c_min directly, it's probably more simple
    //       to calculate (maximum) move_duration for each axis, like done for
    //       ACCELERATION_TEMPORAL above. This should make re-calculating the
    //       allowed F easier.
    c_limit = 0;
    for (i = X; i < AXIS_COUNT; i++) {
      c_limit_calc = (delta_um[i] * 2400L) /
                     dda->total_steps * (F_CPU / 40000) /
                     pgm_read_dword(&maximum_feedrate_P[i]);
      if (c_limit_calc > c_limit)
        c_limit = c_limit_calc;
    }

    dda->c_min = move_duration / dda->endpoint.F;
      if (dda->c_min < c_limit) {
        dda->c_min = c_limit;
        dda->endpoint.F = move_duration / dda->c_min;
      }

      // Lookahead can deal with 16 bits ( = 1092 mm/s), only.
      if (dda->endpoint.F > 65535)
        dda->endpoint.F = 65535;

      // Acceleration ramps are based on the fast axis, not the combined speed.
      dda->rampup_steps =
        acc_ramp_len(muldiv(dda->fast_um, dda->endpoint.F, distance),
                     dda->fast_spm);

      if (dda->rampup_steps > dda->total_steps / 2)
        dda->rampup_steps = dda->total_steps / 2;
      dda->rampdown_steps = dda->total_steps - dda->rampup_steps;

      #ifdef LOOKAHEAD
        dda->distance = distance;
        dda_find_crossing_speed(prev_dda, dda);
        // TODO: this should become a reverse-stepping through the existing
        //       movement queue to allow higher speeds for short moves.
        //       dda_find_crossing_speed() is required only once.
        dda_join_moves(prev_dda, dda);
        dda->n = dda->start_steps;
        if (dda->n == 0)
          dda->c = pgm_read_dword(&c0_P[dda->fast_axis]);
        else
          dda->c = (pgm_read_dword(&c0_P[dda->fast_axis]) *
                    int_inv_sqrt(dda->n)) >> 13;
        if (dda->c < dda->c_min)
          dda->c = dda->c_min;
      #else
        dda->n = 0;
        dda->c = pgm_read_dword(&c0_P[dda->fast_axis]);
      #endif

    } /* ! dda->total_steps == 0 */

    if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
        serial_writestr_P(PSTR("] }\n"));

    // now prepare speed values for dc motor
    if (dda->waitfor)
    {
        // calculate dc motor speed according to distance,
        // this is done by linear interpolation of speed curve divided into two line intervals
        #define MAX_JUMP_LENGTH_JOINT MAX_JUMP_LENGTH/4
        int16_t dc_motor_speed;
        int16_t half_margin_max_speed = margin_max_speed/2;
        if (distance < MAX_JUMP_LENGTH_JOINT * 1000)
            // small distances has a steep speed curve
            dc_motor_speed = half_margin_max_speed + ((margin_max_speed - half_margin_max_speed) * ((int32_t)(distance - MAX_JUMP_LENGTH_JOINT * 1000) / (0 - MAX_JUMP_LENGTH_JOINT))) / 1000;
        else
            // longer distances curve looks more like a horizontal line
            dc_motor_speed = MIN_MOTOR_SPEED + ((half_margin_max_speed - MIN_MOTOR_SPEED) * ((int32_t)(distance - MAX_JUMP_LENGTH * 1000) / (MAX_JUMP_LENGTH_JOINT - MAX_JUMP_LENGTH))) / 1000;
		if(dc_motor_speed > margin_max_speed) dc_motor_speed = margin_max_speed;
		else if(dc_motor_speed < MIN_MOTOR_SPEED) dc_motor_speed = MIN_MOTOR_SPEED;

		if(dc_motor_speed > 0)
		{
			if(prev_dda)
			{
				// start spinning motor slowly
				if(prev_dda->dc_motor_speed == 0)
					dc_motor_speed = MIN_MOTOR_SPEED;
				// speed up, decrease current dda speed if needed
				else if(dc_motor_speed - prev_dda->dc_motor_speed > JUMP_MOTOR_SPEED_DIFF_MAX )
					dc_motor_speed = prev_dda->dc_motor_speed + JUMP_MOTOR_SPEED_DIFF_MAX;
				// slow down, decrease previous dda speed if needed
				else if(prev_dda->dc_motor_speed - dc_motor_speed > JUMP_MOTOR_SPEED_DIFF_MAX_SLOWDOWN )
					queue_set_prev_dc_motor(dc_motor_speed);
			}
			else // no previous movement, start slowly as well
				dc_motor_speed = MIN_MOTOR_SPEED;
		}
		dda->dc_motor_speed = dc_motor_speed;
	}
	// other kind of movement requires motor to be stopped
	else
	{
		dda->dc_motor_speed = 0;
		queue_set_prev_dc_motor(MIN_MOTOR_SPEED);
	}
	// end of motor speed planner

    // next dda starts where we finish
    memcpy(&startpoint, &dda->endpoint, sizeof(TARGET));
    prev_dda = dda;
}


void dda_start(DDA *dda) {
    // called from interrupt context: keep it simple!

        // apply dc motor speed
        uint8_t queue_elements = queue_current_size();
        if(dda->dc_motor_speed == 0 )
        {
			// turn off motor for jump move after current one stitch
			if (desired_speed > 0)
            {
				stop_dc_motor(1);
                dda->waitfor = 1;
            }
		}
		else
		{
			// slow down on almost empty buffer
			if(queue_elements < 4)
			{
                if(queue_elements < 2)
                    desired_speed = MIN_MOTOR_SPEED;
                else
                {
                    desired_speed = dda->dc_motor_speed / 2;
                    if(desired_speed < MIN_MOTOR_SPEED)
                        desired_speed = MIN_MOTOR_SPEED;
                }
			}
			else // just use planned speed value from dda
				desired_speed = dda->dc_motor_speed;
		}

        if (dda->endstop_check)
            endstops_on();
        #ifdef TRIGGERED_MOVEMENT
        // if stepper movement dda is not allowed yet
        else if(dda->waitfor)
            return;
        #endif

        // buffer is empty, this is probably last move, stop now
        if(queue_elements == 0)
            stop_dc_motor(0);

  if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
    #ifdef STEPS_PER_M_Z
        sersendf_P(PSTR("Start: X %lq  Y %lq  Z %lq  F %lu\n"),
               dda->endpoint.axis[X], dda->endpoint.axis[Y],
               dda->endpoint.axis[Z], dda->endpoint.F);
    #else
        sersendf_P(PSTR("Start: X %lq  Y %lq  F %lu\n"),
               dda->endpoint.axis[X], dda->endpoint.axis[Y],dda->endpoint.F);
    #endif

        // get ready to go
        //psu_timeout = 0;

        // set direction outputs
        x_direction(dda->x_direction);
        y_direction(dda->y_direction);
        #ifdef STEPS_PER_M_Z
        z_direction(dda->z_direction);
        #endif
        #ifdef STEPS_PER_M_E
        e_direction(dda->e_direction);
        #endif

        // initialise state variable
        move_state.counter[X] = move_state.counter[Y] =
        #ifdef STEPS_PER_M_Z
        move_state.counter[Z] =
        #endif
        #ifdef STEPS_PER_M_E
        move_state.counter[E] =
        #endif
        -(dda->total_steps >> 1);
        memcpy(&move_state.steps[X], &dda->delta[X], sizeof(uint32_t) * 4);
        move_state.endstop_stop = 0;

        move_state.step_no = 0;

        // ensure this dda starts
        dda->live = 1;

        // set timeout for first step
        timer_set(dda->c, 0);
    // else just a speed change, keep dda->live = 0

    current_position.F = dda->endpoint.F;
}

void dda_step(DDA *dda) {

  if (move_state.steps[X]) {
    move_state.counter[X] -= dda->delta[X];
    if (move_state.counter[X] < 0) {
            x_step();
      move_state.steps[X]--;
      move_state.counter[X] += dda->total_steps;
        }
    }
  if (move_state.steps[Y]) {
    move_state.counter[Y] -= dda->delta[Y];
    if (move_state.counter[Y] < 0) {
            y_step();
      move_state.steps[Y]--;
      move_state.counter[Y] += dda->total_steps;
        }
    }

#ifdef STEPS_PER_M_Z
if (move_state.steps[Z]) {
    move_state.counter[Z] -= dda->delta[Z];
    if (move_state.counter[Z] < 0) {
            x_step();
      move_state.steps[Z]--;
      move_state.counter[Z] += dda->total_steps;
        }
    }
#endif
#ifdef STEPS_PER_M_E
if (move_state.steps[E]) {
    move_state.counter[E] -= dda->delta[E];
    if (move_state.counter[E] < 0) {
            x_step();
      move_state.steps[E]--;
      move_state.counter[E] += dda->total_steps;
        }
    }
#endif
        move_state.step_no++;



  // If there are no steps left or an endstop stop happened, we have finished.
  //
  // TODO: with ACCELERATION_TEMPORAL this duplicates some code. See where
  //       dda->live is zero'd, about 10 lines above.
  if ((move_state.steps[X] == 0 && move_state.steps[Y] == 0
    #ifdef STEPS_PER_M_Z
        && move_state.steps[Z] == 0
    #endif
    #ifdef STEPS_PER_M_E
        && move_state.steps[E] == 0
    #endif
      )  || (move_state.endstop_stop && dda->n <= 0)) {
        dda->live = 0;
        dda->done = 1;
        #ifdef LOOKAHEAD
        // If look-ahead was using this move, it could have missed our activation:
        // make sure the ids do not match.
        dda->id--;
        #endif
        // No need to restart timer here.
        // After having finished, dda_start() will do it.
    }
  else {
        //psu_timeout = 0;
      timer_set(dda->c, 0);
  }

    // turn off step outputs, hopefully they've been on long enough by now to register with the drivers
    // if not, too bad. or insert a (very!) small delay here, or fire up a spare timer or something.
    // we also hope that we don't step before the drivers register the low- limit maximum speed if you think this is a problem.
    unstep();
}

void dda_clock() {
  DDA *dda;
  static DDA *last_dda = NULL;
  uint8_t endstop_trigger = 0;

  uint32_t move_step_no, move_c;
  int32_t move_n;
  uint8_t recalc_speed;
  uint8_t current_id ;


  dda = queue_current_movement();
  if (dda != last_dda) {
    move_state.debounce_count_x =
    move_state.debounce_count_y =
    #ifdef STEPS_PER_M_Z
    move_state.debounce_count_z =
    #endif
    0;
    last_dda = dda;
  }

  if (dda == NULL)
    return;

  // Caution: we mangle step counters here without locking interrupts. This
  //          means, we trust dda isn't changed behind our back, which could
  //          in principle (but rarely) happen if endstops are checked not as
  //          endstop search, but as part of normal operations.
  if (dda->endstop_check && ! move_state.endstop_stop) {
    #ifdef X_MIN_PIN
    if (dda->endstop_check & 0x01) {
      if (x_min() == dda->endstop_stop_cond)
        move_state.debounce_count_x++;
      else
        move_state.debounce_count_x = 0;
      endstop_trigger = move_state.debounce_count_x >= ENDSTOP_STEPS;
    }
    #endif
    #ifdef X_MAX_PIN
    if (dda->endstop_check & 0x02) {
      if (x_max() == dda->endstop_stop_cond)
        move_state.debounce_count_x++;
      else
        move_state.debounce_count_x = 0;
      endstop_trigger = move_state.debounce_count_x >= ENDSTOP_STEPS;
    }
    #endif

    #ifdef Y_MIN_PIN
    if (dda->endstop_check & 0x04) {
      if (y_min() == dda->endstop_stop_cond)
        move_state.debounce_count_y++;
      else
        move_state.debounce_count_y = 0;
      endstop_trigger = move_state.debounce_count_y >= ENDSTOP_STEPS;
    }
    #endif
    #ifdef Y_MAX_PIN
    if (dda->endstop_check & 0x08) {
      if (y_max() == dda->endstop_stop_cond)
        move_state.debounce_count_y++;
      else
        move_state.debounce_count_y = 0;
      endstop_trigger = move_state.debounce_count_y >= ENDSTOP_STEPS;
    }
    #endif


    // If an endstop is definitely triggered, stop the movement.
    if (endstop_trigger) {
      #ifdef MILD_HOMING
        // For always smooth operations, don't halt abruptly,
        // but start deceleration here.
        ATOMIC_START
          move_state.endstop_stop = 1;
          move_step_no = dda->total_steps - move_state.steps[dda->fast_axis];

          if (move_step_no > dda->rampup_steps) {  // cruising?
            move_step_no = dda->total_steps - dda->rampdown_steps;
          }

          dda->rampdown_steps = move_step_no;
          dda->total_steps = move_step_no * 2;
          move_state.steps[dda->fast_axis] = move_step_no;
        ATOMIC_END

        // Not atomic, because not used in dda_step().
        dda->rampup_steps = 0; // in case we're still accelerating
      #else
        dda->live = 0;
      #endif

      endstops_off();
    }
  } /* ! move_state.endstop_stop */


    // For maths about stepper speed profiles, see
    // http://www.embedded.com/design/mcus-processors-and-socs/4006438/Generate-stepper-motor-speed-profiles-in-real-time
    // and http://www.atmel.com/images/doc8017.pdf (Atmel app note AVR446)
    ATOMIC_START
      current_id = dda->id;
      move_step_no = move_state.step_no;
      // All other variables are read-only or unused in dda_step(),
      // so no need for atomic operations.
    ATOMIC_END

    recalc_speed = 0;
    if (move_step_no < dda->rampup_steps) {
      #ifdef LOOKAHEAD
        move_n = dda->start_steps + move_step_no;
      #else
        move_n = move_step_no;
      #endif
      recalc_speed = 1;
    }
    else if (move_step_no >= dda->rampdown_steps) {
      #ifdef LOOKAHEAD
        move_n = dda->total_steps - move_step_no + dda->end_steps;
      #else
        move_n = dda->total_steps - move_step_no;
      #endif
      recalc_speed = 1;
    }
    if (recalc_speed) {
      if (move_n == 0)
        move_c = pgm_read_dword(&c0_P[dda->fast_axis]);
      else
        // Explicit formula: c0 * (sqrt(n + 1) - sqrt(n)),
        // approximation here: c0 * (1 / (2 * sqrt(n))).
        // This >> 13 looks odd, but is verified with the explicit formula.
        move_c = (pgm_read_dword(&c0_P[dda->fast_axis]) *
                  int_inv_sqrt(move_n)) >> 13;

      // TODO: most likely this whole check is obsolete. It was left as a
      //       safety margin, only. Rampup steps calculation should be accurate
      //       now and give the requested target speed within a few percent.
      if (move_c < dda->c_min) {
        // We hit max speed not always exactly.
        move_c = dda->c_min;

        // This is a hack which deals with movements with an unknown number of
        // acceleration steps. dda_create() sets a very high number, then,
        // but we don't want to re-calculate all the time.
        // This hack doesn't work with lookahead.
        #ifndef LOOKAHEAD
          dda->rampup_steps = move_step_no;
          dda->rampdown_steps = dda->total_steps - dda->rampup_steps;
        #endif
      }

      // Write results.
      ATOMIC_START
        /**
          Apply new n & c values only if dda didn't change underneath us. It
          is possible for dda to be modified since fetching values in the
          ATOMIC above, e.g. when a new dda becomes live.

          In case such a change happened, values in the new dda are more
          recent than our calculation here, anyways.
        */

        if (current_id == dda->id) {
          dda->c = move_c;
          dda->n = move_n;
        }
      ATOMIC_END
    }
    else {
      ATOMIC_START
        if (current_id == dda->id)
          // This happens only when !recalc_speed, meaning we are cruising, not
          // accelerating or decelerating. So it pegs our dda->c at c_min if it
          // never made it as far as c_min.
          dda->c = dda->c_min;
      ATOMIC_END
    }
}

/// update global current_position struct
void update_current_position() {
    DDA *dda = mb_tail_dda;
  uint8_t i;

  // Use smaller values to adjust to avoid overflow in later calculations,
  // (STEPS_PER_M_X / 1000) is a bit inaccurate for low STEPS_PER_M numbers.
  static const axes_uint32_t PROGMEM steps_per_mm_P = {
    ((STEPS_PER_M_X + 500) / 1000),
    ((STEPS_PER_M_Y + 500) / 1000)
    #ifdef STEPS_PER_M_Z
    ,((STEPS_PER_M_Z + 500) / 1000)
    #endif
    #ifdef STEPS_PER_M_E
    ,((STEPS_PER_M_E + 500) / 1000)
    #endif
  };

    if (queue_empty()) {
    for (i = X; i < AXIS_COUNT; i++) {
      current_position.axis[i] = startpoint.axis[i];
    }
    }
    else if (dda->live) {
    for (i = X; i < AXIS_COUNT; i++) {
      current_position.axis[i] = dda->endpoint.axis[i] -
          (int32_t)get_direction(dda, i) *
          // Should be: move_state.steps[i] * 1000000 / steps_per_m_P[i])
          // but steps[i] can be like 1000000 already, so we'd overflow.
          // Unfortunately, using muldiv() overwhelms the compiler.
          // Also keep the parens around this term, else results go wrong.
          ((move_state.steps[i] * 1000) / pgm_read_dword(&steps_per_mm_P[i]));
    }
#ifdef STEPS_PER_M_E
    if (dda->endpoint.e_relative)
      current_position.axis[E] =
          (move_state.steps[E] * 1000) / pgm_read_dword(&steps_per_mm_P[E]);
#endif
        // current_position.F is updated in dda_start()
    }
}

sensors_control.c

C/C++
Modified sensors_control.c file. Set the registers such that the PWM of timer0 works for ATMega328/168 and redefined the output flags.
#include "sensors_control.h"

uint8_t last_direction = 0;
uint8_t stop_motor_flag = 0;
uint8_t interrupt_ticks = 0;
uint16_t motor_pulses = 0;

int32_t ki, kp;
int16_t error_speed_sum;
int16_t desired_speed, error_speed;
int16_t margin_max_speed = DEFAULT_MOTOR_SPEED;

/// Init INT0 and INT1 interrupts for optical sensors, init pwm and timers for dc motor speed controller
void sensing_init()
{
	PULL_OFF(PD2);
    PULL_OFF(ENCODER_PIN_A);
    PULL_OFF(ENCODER_PIN_B);
    SET_INPUT(PD2);
    SET_INPUT(ENCODER_PIN_A);
    SET_INPUT(ENCODER_PIN_B);

  #if defined (__AVR_ATmega32U4__) || defined (__AVR_ATmega32__) || \
            defined (__AVR_ATmega16__)
	// externally trigged interrupts setup
    MCUCR |= MASK(ISC00) | MASK(ISC11) | MASK(ISC10); /* INT0 - any change, INT1 - raising edge */
    GICR |= MASK(INT0) | MASK(INT1);        /* enable INT0 and INT1 */

    // setup PWM timers: fast PWM
	// Warning: these are not consistent across all AVRs
	TCCR0 = MASK(WGM01) | MASK(WGM00) | MASK(COM01);
	// pwm frequency
	#ifndef FAST_PWM
		TCCR0 |= MASK(CS02); // F_CPU / 256
	#else
		TCCR0 |=  MASK(CS01); // F_CPU / 8
	#endif

	#define MOTOR_PWM OCR0
	OCR0 = 0;
	PB4_DDR = 0xFF;

	// timer 1 is used for stepping

    TCCR2 = MASK(CS20) | MASK(CS21) | MASK(CS22); // F_CPU / 256 / 1024
    OCR2 = 0;
    // enable timer overflow int
    TIMSK |= MASK(TOIE2);
  #else
    // externally trigged interrupts setup
    EICRA |= MASK(ISC00) | MASK(ISC11) | MASK(ISC10); /* INT0 - any logic change INT1 - raising edge */
    EIMSK |= MASK(INT0) | MASK(INT1);        /* enable INT0 and INT1 */

	// timer 1 is used for stepping

    // PWM frequencies in TCCR2B, see page 156 of the ATmega644 reference.
    TCCR2A = 0;
    TCCR2B = MASK(CS20) | MASK(CS21) | MASK(CS22); // F_CPU / 256 / 1024

    OCR2A = 0;
    OCR2B = 0;

    // pwm frequency
    #ifndef FAST_PWM
		TCCR0B =  (TCCR0B & 0b11111000) | 0x04;
	#else
		TCCR0B =  (TCCR0B & 0b11111000) | 0x02;
	#endif
		// init of timer 0 for pwm motor control. ouput pin is PD6
		TCCR0A = MASK(WGM01) | MASK(WGM00) | MASK(COM0A1);
		#define MOTOR_PWM OCR0A
		OCR0A = 0;
		OCR0B = 0;
		SET_OUTPUT(PD6);

    // enable timer overflow int
    TIMSK2 = MASK(TOIE2);
  #endif

    error_speed = desired_speed = error_speed_sum = 0;
	kp = KP_FACTOR;
	ki = KI_FACTOR;

}
void set_dc_motor_speed(int16_t value)
{
	desired_speed = value;
}

/// sets machine maximal work speed
void set_dc_motor_speed_margin(int16_t value)
{
	if (value <= MAX_MOTOR_SPEED)
	{
		if (value >= MIN_MOTOR_SPEED)
			margin_max_speed = value;
		else
			margin_max_speed = MIN_MOTOR_SPEED;
	}
	else
		margin_max_speed = MAX_MOTOR_SPEED;
}

/// activates stop of the the motor on needle downwards interrupt
/// mode 0 - stop on first interrupt, any other value - stop on next one interrupt
void stop_dc_motor(uint8_t mode)
{
    if (stop_motor_flag || desired_speed == 0) return;
    if (mode)
    {
        desired_speed = MIN_MOTOR_SPEED;
        stop_motor_flag = 2;
    }
    else
    {
        desired_speed = MIN_MOTOR_SPEED/2;
        stop_motor_flag = 1;
    }
}

/// PI control update procedure
void update_dc_motor()
{
	int16_t pwm_pulse;
	if (desired_speed > 3 )
	{
		if(interrupt_ticks <= 0) return;
		// ticking interrupt is called at about 61 Hz rate
		uint8_t time_interval = (F_CPU / 256 / 1024 / interrupt_ticks);
		//calculate motor speed, unit is decimal rpm
		int16_t pv_speed = ( 60 * (motor_pulses*POINT_SHIFT/PULSES_PER_TURN) * time_interval ) / POINT_SHIFT;

		interrupt_ticks = 0;
		motor_pulses = 0;
		error_speed = desired_speed - pv_speed;
		pwm_pulse = (int32_t)(((int32_t) error_speed*kp) + ((int32_t) error_speed_sum*ki)) / POINT_SHIFT;
		error_speed_sum += error_speed; //sum of error
		// integral limitation
		if (error_speed_sum >ACCUMULATOR_LIMIT) error_speed_sum = ACCUMULATOR_LIMIT;
		else if (error_speed_sum < -ACCUMULATOR_LIMIT) error_speed_sum = -ACCUMULATOR_LIMIT;
   }
   else
   {
		error_speed = 0;
		error_speed_sum = 0;
		pwm_pulse = 0;
		interrupt_ticks = 0;
   }
	// set new motor power
	if (pwm_pulse < 255 && pwm_pulse > 0)
	{
		MOTOR_PWM = pwm_pulse;
	}
   else
   {
		if (pwm_pulse>255)
		{
			MOTOR_PWM = 255;
		}
		else
		{
			MOTOR_PWM = 0;
		}
   }
}

/// needle interrupt
ISR(INT1_vect)
{
	// check if second sensor is trigged to estimate needle direction
	if(direction_encoder())
	{
		// false alarm, last interrupt was about the same direction
		if(last_direction)
			return;
		last_direction = 1;
        if(stop_motor_flag == 2)
        {
            --stop_motor_flag;
            desired_speed = MIN_MOTOR_SPEED/2;
        }
		if(mb_tail_dda && !mb_tail_dda->live && !stop_motor_flag)
		{
			mb_tail_dda->waitfor = 0;
			dda_start(mb_tail_dda);
			sei();
		}
        //serial_writestr_P(PSTR("int1 up\n"));
	}
	else // needle goes downwards
	{
		if(!last_direction)
			return;
		last_direction = 0;
		//TODO: stop any movement currently in progress
		if(stop_motor_flag == 1)
		{
			desired_speed = 0;
			stop_motor_flag = 0;
            if(mb_tail_dda && !mb_tail_dda->live)
            {
                mb_tail_dda->waitfor = 0;
                dda_start(mb_tail_dda);
                sei();
            }
		}
        //serial_writestr_P(PSTR("int1 down\n"));
	}
}

/// motor rotary encoder interrupt for speed measurement
ISR(INT0_vect)
{
	++motor_pulses;
}

/// timer tick interrupt
ISR(TIMER2_OVF_vect)
{
	++interrupt_ticks;
}

Credits

SpaceForOne

SpaceForOne

1 project • 10 followers
Thanks to markol.

Comments