Foreword
Word of caution
Introduction
Overview
Mechanics
Electronics
Software
Result
Miscellaneous
Closing Remarks

Published May 21, 2023 © 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 days10,981

Things used in this project

Hardware components

Arduino UNO

Story

Foreword

The goal of this article is to document my approach or my realization of markol's project Embroiderino. The heavy lifting was done by him. His documentation is amazing and sufficient to realize an embroidery machine. Still I think that the documentation of my take on his project might be useful for

people using the same or a similar sewing machine like mine
peope using the popular GRBL shield for Arduino since this way the PCB work gets simpler and more approachable.

Additionally I will mention details which I had to find workarounds for or which made my life harder such that you may have a little bit less debugging to do. :)

So whom is this article for? For people following markol's tutorial and looking for some additional inspiration. If you you are using a BERNINA 730/731/732 you will also be able to profit of all the 3D files.

What is this article NOT? A step-by-step tutorial on how to build this machine. This article is an addition to the tutorial by markol.

Word of caution

The original project uses circuits that work with mains voltage which can be lethal. If you lack proper certification and/or experience to handle such voltages do not attempt this project in the way it is presented. Attempt this project only at your own responsibility and use this article for entertainment purposes only rather than for education.

Introduction

An embroidery machine has been on my bucket list for quite some time because it would complement my sewing projects quite nicely. When I then could get hold of a beautiful old BERNINA 732 for free I had to start this project.There are quite some open source projects for DIY embroidery machines to choose from. My personal requirements were

Embroider at speeds above 400 stitches/minute
Use the motor of the sewing machine
Have as much of the sensors and wiring hidden inside the machine for a clean look

After some searching I found the website of markol with his take on building an embroidery machine. His implementation allows for fast stitching and uses the machine's built in motor. Perfect!

Overview

The project can be roughly divided into four areas:

The XY gantry for the hoop movement and driving of the associated stepper motors
The high voltage driving circuit for the motor of the sewing machine
The sensing of the sewing machine's speed and position
The microcontroller coordinating everything including the UART interface to the computer

In the following I will adopt the sections as they were in markol's blog.

Mechanics
Electronics
Software

1 / 3 • Top view of machine

Mechanics

For the mechanics you basically need to build a plotter. A coreXY architecture is beneficial because it keeps the moving gantry lightweight and the motors out of the way. Markol has his own take on this. I used the frame by Lehavier mainly because I started with this and I was not aware of markol's project by then. I think I would prefer markol's approach now because the fabric of what you are emboidering can jam the gantry of Lehavier's frame if it gets between the axes. On the other hand the framed design allows for a smaller overall footprint of the machine. The coreXY frame is suspended on four holders which bring the frame up to the required height (contained in the 3D files). Note that I flipped the frame such that the timing belts are on the underside of the frame. The thought behind this was that the fabric will not interfere with or jam the belts. The disadvantage of this is that a part connecting the belts on the underside to the linear rail on the top is necessary (Belt_to_block.STL). This (gray) part inroduces additional elasticity which worsens the quality for larger hoops and/or faster stitching speeds. This part might be replaced by one out of metal in the future.

1 / 2 • Cable chain and visible gray connection part from belts on the bottom to carriage on the top.

You might have noticed the cable chain on the x axis. It is based upon the design of dani_b. I designed the end pieces for the chain to be mounted onto the 2020 extrusion and glued to the the y gantry respectively. The cable chain is only for the two limit switches on the y gantry. Is it strictly necessary? No. Does it look cool? Yes. Be aware that the firmware only uses the limit switches for homing and not for limiting movements. I rarely use the homing feature so I do not know If I would put in the same effort into the limit switches again.

I used embroidery hoops from aliexpress which are meant for Brother Innovis machines and designed adapters for them.

Some general informations about the physical realization:

I used two 20cm MGN12H linear rails for the x axis and one 30cm MGN12H linear rail for the y axis. This is also the maximum I am able to embroider.
The sewing machine and the plotter are mounted on a 72cm x 50cm x 15mm film coated plywood. I wanted the contraption to be compact and easily movable. A frame under the plywood makes picking it up easier and allows to route cables unter the plywood.
The sewing machine and the main electronics box are not screwed down but stand on 3D printed feet which hold them firmly in place.
The electronics box has the dimensions of 14.5x14.5x7cm.

Electronics

The way I installed the sensors and electronics was influenced mainly by my goal of integrating all sensors neatly into the machine.

1 / 2 • Overview over the internals of the machine. The shaft speed sensor is to the right, the needle sensor is in the hatch where the old light was (not visible).

To install the needlesensor the plate formerly housing the light bulb was removed inculding the light bulb and the corresponding wire. The studs of the screws were reused to hold a plate onto which the light barrier and the corresponding circuitry are fastened. The small plate triggering the light barrier is clipped onto the needle shaft. This solution works very well but has to be designed specifically for the machine at hand. The position of the light barrier can be fine adjusted by moving the light sensor up and down (attaching it with isolation tape allows for easy adjustment) and trimming the size of the small plate. This solution proved to work very reliably and stable. I added a salvaged high power LED from a headlamp to the bottom of the needle sensor PCB sinced there are 5V present on the PCB anyway. It is used to shed light on the piece you are working on.

1 / 3 • Needle sensor PCB mounted on the 3D printed backplate. Small flag for interrupting the light barrier fastened to the needle shaft is also visible.

The shaft speed sensor is located on the right hand side of the housing at the wall just inside after the hand turning wheel. I designed the disc for the rotary encoder to fit on the narrow space to the left of the belt driving the hand turning wheel/the shaft. The disc is printed in two parts and fastened to the metal shoulder terminating the hand turning wheel to the left using just a drop of CA glue. I went for 72 fins/slots which is a good trade-off between being fine grained and the fins not being fragile. As the sensor I used one of these cheap speed sensor boards which also have an OpAmp (often a LM393) on board. I attatched it preliminarily to the case using removable poster putty. This way you can easily adjust the position of the light barrier. I originally planned to use a more permanent putty but the poster putty holds up just fine even after quite some embroidering.

1 / 3 • Drive shaft speed sensor including encoder disk mounted on the hand wheel.

The wires beloning to each of the sensor units are routed inside the case of the machine to the junctionbox. My sewing machine does not need to serve another purpose than being a part of the embroidery machine but nevertheless I did not want to drill holes into the main body. That is why I designed a spacer body to go between the main body and the upper thread holder. The spacer provides just enough offset to house a 5 pin socket to connect the sensors to the main PCB. Like that you just need longer M4 screws and you can attatch the whole assemby neatly and without drilling holes to the main body. From there the sensors are connected to the box housing the main PCB using a shielded (!) cable.

1 / 3 • Junction box closed.

The motor of my machine is an universal motor. It is called universal because it can be driven by AC or DC. This can be easily identified by the replacable brushes and the windings on the rotor as well as on the stator which are connected in series. The driving circuit from markol drives the motor with a DC current. His solution is really nice and has several advantages over the "classical" approach of phase angle control which was also implemented in the pedal of the machine. Be aware that the gate driving circuit used does not allow for much higher switching frequencies than the approx. 500Hz used in this project. Driving a universal motor with DC has several advantages, some of which are described nicely in this application note for the interested reader. The driving circuit providing DC current also allows you to use it with brushed permanent magnet DC motors if your sewing machine happens to have such a motor. It will however not work with newer brushless DC motors as they require a different kind of driving circuitry.The cables attached directly to the motor are routed through the case to the main electronics box on the front.

1 / 2 • Inside of main electronics box

In front of the sewing machine is the main electronics box. It houses the motor driving circuit, the sensor interface aka the schmitt triggers and an Arduino UNO with the CNC shield and the stepper motor drivers stacked upon it. Markol fabricated his own PCB and used a different uC than the one present on the Arduino UNO. I had an UNO and a CNC shield laying around so I used them. The stepper drivers are A4988s which are sufficient for the task at hand. The side panel of the box houses an AC input socket, an AC switch, a cutout to access the USB port of the Arduino, a low voltage input (XT60 connector) as well as a NC push button on the top left. The push button is in series with the low voltage supply. This lets you easily interrupt the supply of the steppers which is really handy to quickly reposition the hoop.

1 / 2 • Side panel of main electronics box. Note that the power wires received double isolation later on.

When you want to tackle such a project it is important to consider electromagnetic interference (EMI) because we need good sensor data. The main source of EMI in this setup is the universal motor. It has carbon brushes which ride on a sectioned slip ring. On these brushes there will be sparks. So even tough the running motor sounds acoustically like a purring cat it is sounding like a dying one in the electromagnetic space, emitting HF-EMI. This in turn can and will be picked up by our circuitry and wiring and distort our sensor data. I will list some measures you can take to reduce the EMI you circuit has to endure:

Check the filter capacitor at the motor. Machines running universal motors will always have one. That is the part which has most often failed in old machines. When in doubt replace it, else you are fighting a lost battle.
Twist all the unshielded wires and use a shielded cable for making connections. Pull the shield to a defined potential.
Most older machines I have seen are class II devices hence the metal body will be floating and its EMI shielding potential will be limited. This may be improved if the metal housing is pulled to a defined potential. The same can or should be done for the (metal) enclosure of the main electronics.
If you are still experiencing conducted EMI through the wires running to the motor you can employ a common mode choke.
Having the gate drive interface optocoupler on your low voltage side reduces the exposure of your low voltage side to noise originating from the high voltage switching.

I chose the location of my main electronics box to be as far away from the motor as possible. The drawback of this way is the rather long sensor cable running staight over the motor. Hence the importance of a shielded wire.One drawback of choosing the Arduino UNO + CNC shield way of realizing this project is that you are at least encouraged to use jumper wires to connect everything up. This setup/mess with large loops of wire in the electronics box is a warm welcome for trouble with EMI so make sure you shield everything properly.

Important: The schmitt triggers need to be close to the main board such that any Interference picked up on the way from sensor to the main board is also filtered.

Arduino & CNC shield setup
The usage of the CNC shield simplifies the PCBs we have to build but there are some things we have to consider.

We are forced to use the pins PD2 and PD3 for the external interrupts since these are the only ones providing this feature on the ATMega328p. Unfortunately these are also the ones used for the Step Pulse X and Y respectively on the CNC shield. Thus we need to use the Z and the A axis for our motors. Luckily the design of the CNC shield allows us to use the A axis with the pins PB4 (Step) and PB5 (Dir). To make use of this feature you have to place the respective jumpers.
Another pin which is reserved is the PD6. It is the output for the OC0A which is used to generate the PWM signal for the motor driver. It corresponds to the the "Y-axis Direction" pin on the CNC shield.

Summarizing the connections to the CNC shield are as follows (the last column is the name of the pin on the CNC shield):

X_STEP_PIN/X_DIR_PIN -> PB4/PB5 -> Axis A STEP/DIR
Y_STEP_PIN/Y_DIR_PIN -> PD4/PD7 -> Axis Z STEP/DIR
MOTOR_ENCODER_PIN -> PD2 -> Axis X STEP
ENCODER_PIN_A -> PD3 -> Axis Y STEP
ENCODER_PIN_B -> PC3 -> Coolant Enable (somewhat arbitrarily)
PWM_OUT_PIN -> PD6 -> Axis Y DIR

You can check out my config.h for the rest of the pins. Note that MOTOR_ENCODER_PIN and PWM_OUT_PIN are hardcoded in the sensors_control.c file.

Software

The software workflow is as follows: You create a.txt or a.gcode file using a suitable program. Markol recommends embroidermodder, I used InkStitch (a plugin for InkScape) because I am used to InkScape. This file gets loaded into the control app from the emroiderino repository which then sends the commands via USB to the uC. The uC is running teathimble.

InkstitchAs mentioned, InkStitch is a free open source plugin to the also free open source program InkScape. To create files readable for the host application you have to save them as.gcode files. For that purpose you can enter custom GCODE commands to match the ones of the embroiderino project. However this does not work perfectly yet:The current release of Inkstitch has some issues with the custom GCODE output. You can alter the STITCH command but not the MOVE command. The MOVE command being G1 by default in inkscape you will get a file with only G1 commands. Also it does not insert the color change commands at all. Up until now I just added/altered the commands manually afterwards which is of course not a sustainable way of solving the issues. They of course can be resolved by adjusting either programs. It just has not been done yet.Side Note: This is not documented on the teathimble/embroiderino site but to perform a color change the command "M6 R? G? B?" is expected (so M6 R245.0 G111.0 B3.0 for example).

control_appThe control app works nicely for me. Up to some hickups like the pause button not working from time to time everything has been working flawlessly.

teathimbleAlso a very nice piece of code and the author foresaw compatibility with the ATMega328p which sits on the Arduino UNO. However there were two issues which gave me headache:

The Waveform Generation Mode and the Compare Output Mode bits are not set for the ATMega328p leading to no ouput signal on the OC0A output pin (PD6 for ATMega328p). There was also a line declaring the complete D bank as outputs thus disabling the sensor inputs.
I observed inconsistent movement of the x axis when using the jog buttons of the control app. I could narrow it down to the variable startpoint_steps in motor.c. It seems that a different process/variable is colliding in memory with it leading to inconsistent steps taken. I found that removing startpoint_steps from the BSS section for the ATMega328p works out fine altough this is not the most elegant solution.

These bugs have been removed to an extent that it is working for the ATMega328p. I issued a merge request but until that is approved you can copy the attached files and replace the ones from the repo with them on your local machine. Namely motor.c and sensors_control.c need to be replaced. Alternatively you can checkout my fork of teathimble.

Result

All in all the machine works like a charm. At least from an electroics and software perspective it just works. I still have some issues with the upper thread breaking but that is a matter of fine tuning the upper and lower thread tension. I found that it is a fine line between having no tension at all on the upper thread and having too much such that the thread breaks. There is still some tuning to do I guess.

Anyway here is a video where you can see the machine in action:(be aware that the audio is terrible, my mic was covered)

A Demo of the machine in action

The embroidery file is from Low Tech Linux who provides the files for download on his website. I edited the file such that it has less jumps and exported the gcode file to use with my machine. You can find the gcode file attached. Low Tech Linux has amazing tutorials on youtube on how to digitize images and how to use InkStitch; a very valuable ressource if you plan on using InkStitch!

I really do like the results I am getting from the machine. There are some small areas here and there which I am not quite satisfied with but I still have a lot to learn about embroidering.

Outcome of the Demo

Miscellaneous

The STL files are collected on Thingiverse. I collected all the parts specifically for the embroidery machine in one thing and the other ones which might be of use for other projects/replacements are in seperate things, listed below.

Machine specific remarks:

I covered the original sockets for the pedal and the power cable with 3D printed parts. They are also attached with all the other STL files here.
There were two versions of the BERNINA 730 Record (the Models 730/31/32 differ only by the stitch selection) of the upper thread tensioner. Machines up until March 1964 had a pretensioner before the main upper thread tensioning assembly. I have a machine from 1963 and the pretensioner was missing. You will not get good results by installing just a guide. So I designed a pretensioner consisting of some 3D printed parts and some hardware. You can find the files and how-to of the pre-tensioner here.
For embroidery a stitchplate with only a small hole is benificiary. I desigend one for this machine and it is included in the files. Note that you have to remove the hooks of the transporter. You can find the stitch plate here.
One of the caps retaining the carbon brushes was brittle and broke. I designed a replacement part which you can find on thingiverse. You can find it here.

Close up of pretensioner

Closing Remarks

It took me about 6 months to complete the project. It was as fun as it was challenging and I learned a lot! And I have an embroidery machine now! :)If you are interested in embroidery and/or have an old sewing machine which is craving a second life then I am encouraging you to tackle it as well!

Code

#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()
    }
}

#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

1 project • 11 followers

Thanks to markol.

Arduino Based Embroidery Machine