ADI Trinamic Blog

We transform digital information into physical motion.

 
  • Newsletter
  • Blog Ethics
  • Legal/Impressum
  • Main Website
Menu
  • Newsletter
  • Blog Ethics
  • Legal/Impressum
  • Main Website
  • How to drive a stepper motor closed loop with your Arduino Uno using a TMC4361A-EVAL + TMC2130-EVAL

    Today we will wire up a TMC4361A-EVAL + TMC2130-EVAL combination to drive a stepper motor closed loop with an Arduino Uno. The encoder used for this test has a resolution of 10.000 cpr respective a resolution of 40.000.

    Preparation

    For this tutorial the Eselsbruecke got a soldered connection between +5V_USB (pin 5) to +5V_VM (pin 42) to get +5V for the encoder. You can use jumper wires as well but make sure the connection is properly.

    Wiring

    The wiring is very simple. You will need 9 jumper wires. To make the wiring more easy you can print out the TMC5130-EVAL_Pinning.pdf (used from a previous blog entry) and cut out the template to mount it on the connector header of the TMC4361-EVAL (As seen on illustration 4). As a reference you can use the TMC5130-Eval_v15_01_Schematic.pdf. Here you’ll find the signals that are on each pin. The configuration is documented in the comment section of the Arduino code.

    Illustration 3 – Pinheader of TMC4361-EVAL

    Illustration 4 – TMC4361A+TMC2130 wired up with Arduino Uno

    Cable colors of illustration 4
    +5V –> red
    GND –> blue
    SDO –> yellow
    SDI –> orange
    SCK –> white
    CSN –> grey
    DIO0 (DRV_ENN) –> black (Is passed thru to TMC2130-EVAL DRV_ENN pin)
    DIO16 (NFREEZE) –> brown
    CLK16 –> green

    Arduino Code

    The Arduino Code below does not need any additional libraries. The SPI library comes with the Arduino IDE. The program initializes the TMC2130 thru the TMC4361A-EVAL (cover datagram) and the TMC4361 itself. It will rotate a 200 full step motor 2 revolutions to the one and 2 revolutions to the other direction depending on the wiring of the stepper motor. Please use either the TMC4361 and TMC2130 datasheets as a reference for the different registers (Download links below).

    #include <SPI.h>
    #include "TMC4361A_Register.h"
    
    /* The trinamic TMC4361 motor controller operates through an 
     * SPI interface. Each datagram is sent to the device as an address byte
     * followed by 4 data bytes. This is 40 bits (8 bit address and 32 bit word).
     * Each register is specified by a one byte (MSB) address: 0 for read, 1 for 
     * write. The MSB is transmitted first on the rising edge of SCK.
     * 
     * Arduino Uno Pins Eval Board Pins
     * 11 MOSI 32 SPI1_SDI
     * 12 MISO 33 SPI1_SDO
     * 13 SCK 31 SPI1_SCK
     * 8 CS 30 SPI1_CSN
     * 7 DIO 8 DIO0 (DRV_ENN)
     * 9 DIO 23 CLK16
     * 2 DIO 38 NFREEZE
     * GND 2 GND
     * +5V 5 +5V
     */
    
    int chipCS = 8;
    const byte CLOCKOUT = 9;
    int enable = 7;
    int nFreeze = 2;
    
    void setup()
    {
     // put your setup code here, to run once:
     pinMode(chipCS,OUTPUT);
     pinMode(CLOCKOUT,OUTPUT);
     pinMode(enable, OUTPUT);
     pinMode(nFreeze, OUTPUT);
    
     digitalWrite(chipCS,HIGH);
     digitalWrite(enable,LOW);
     digitalWrite(nFreeze,HIGH);
    
     //set up Timer1
     TCCR1A = bit (COM1A0); //toggle OC1A on Compare Match
     TCCR1B = bit (WGM12) | bit (CS10); //CTC, no prescaling
     OCR1A = 0; //output every cycle
    
     SPI.setBitOrder(MSBFIRST);
     SPI.setClockDivider(SPI_CLOCK_DIV8);
     SPI.setDataMode(SPI_MODE3);
     SPI.begin();
    
     Serial.begin(9600);
    
     // General setup of TMC4361 Motion Controller with TMC2130 stepper driver
    
     sendData(0xCF,0x52535400); // reset squirrel
    
     sendData(0x83,0x00540022); // input filter: START and encoder pins
     sendData(0x84,0x8440000d); 
     
     sendData(0x80,0x00007026); // direct-a, direct-bow
     sendData(0x90,0x00060004); // Steplength
     
     sendData(0xED,0x00000080); // Using cover datagram to write to GCONF of TMC2130:
     sendData(0xEC,0x00012200); // diag1=index, pushpull, direct_mode = on --> SPI mode
     
     sendData(0xED,0x00000090); // Using cover datagram to write to IHOLD_IRUN of TMC2130:
     sendData(0xEC,0x00051905); // IHOLD = 29, IHOLDDELAY = 5, IRUN = 25
     
     sendData(0xED,0x00000091); // Using cover datagram to write to TPOWERDOWN of TMC2130:
     sendData(0xEC,0x0000000A); // TZEROWAIT = 10
     
     sendData(0xED,0x00000094); // Using cover datagram to write to TCOOLTHRS of TMC2130:
     sendData(0xEC,0x000000C8); // = 200
     
     sendData(0xED,0x00000095); // Using cover datagram to write to TCOOLTHRS of TMC2130:
     sendData(0xEC,0x00000032); // = 50
     
     sendData(0xED,0x000000EE); // Using cover datagram to write to DCCTRL of TMC2130:
     sendData(0xEC,0x00070025); // 
     
     sendData(0xED,0x000000F0); // Using cover datagram to write to PWMCONF of TMC2130:
     sendData(0xEC,0x000404FF); // DCCTRLSG = 5, DC_TIME = 37
     
     sendData(0xED,0x000000EC); // Using cover datagram to write to CHOPCONF of TMC2130:
     sendData(0xEC,0x00010223); // 
     
     sendData(0xED,0x000000ED); // Using cover datagram to write to COOLCONF of TMC2130:
     sendData(0xEC,0x0100a220); // OFF
    
     sendData(0xA4,0x00000000); // v = 0
     sendData(0xA1,0x00000000); // xactual = 0
     sendData(0xB7,0x00000000); // xtarget = 0
    
     // Closed Loop calibration of TMC4361 Motion Controller
    
     sendData(0xD4,0x00009C40); // Encoder resolution = 40k/rev
    
     sendData(0x9C,0x00FF00FF); // CL_BETA = CL_GAMMA = 255
     
     sendData(0xA0,0x00000004); // hold + position mode
    
     sendData(0xA4,0x00100000); // slow velocity
    
     sendData(0xDC,0x00010000); // cl_p = 1.0
    
     sendData(0x87,0x00400000); // turn on closed loop
     
     sendData(0xB7,0x00000080); // move to full step position
    
     sendData(0x87,0x01400000); // turn on closed loop calibration
    
     sendData(0x87,0x00400000); // turn off closed loop calibration
    
     sendData(0xA4,0x00000000); // v = 0
    
    
     // Setup Closed Loop Operation
     sendData(0xA0,0x00000006); // S-Ramp + POS Mode
     sendData(0xA4, 0x7A12000); // VMAX = 500000 pps
    
     sendData(0xA8,0x00000200); // AMAX
     sendData(0xA9,0x00000200); // DMAX
     
     sendData(0xAD,0x00000100); // bow1
     sendData(0xAE,0x00000100); // bow2
     sendData(0xAF,0x00000100); // bow3
     sendData(0xB0,0x00000100); // bow4
     
     sendData(0xE0,0x00250000); // emf_vmin = 
     sendData(0xE1,0x00450000); // emf_vadd = -> emf_vmax = 
     
     sendData(0xE2,0x00FFFFFF); // emf_vel0_timer
     sendData(0xE3,0x02000864); // enc vel filter settings
     
     sendData(0xDF,0x00000014); // cl_tolerance = 20
     sendData(0xDC,0x00010000); // cl_p = 1.0
     sendData(0xDA,0x000000C8); // cl_vlimit_p = 200
     sendData(0xDB,0x00000032); // cl_vlimit_i = 50
     sendData(0xDD,0x000003E8); // cl_vlimit_diclip = 1000
     sendData(0xDE,0x00100000); // cl_vlimit = 1048576 pps
    
     sendData(0x86,0x0032f064); // cl scaling values
     sendData(0x98,0x00001000); // cl_upscale
     sendData(0x99,0x00100000); // cl_dnscale
     
     sendData(0x87,0x0A400000); // cl with gamma correction and vlimit
     sendData(0x85,0x00000080); // cl scaling on
    
    }
    
    void loop()
    {
     // put your main code here, to run repeatedly:
     sendData(0xB7,0x00019000); // XTARGET = 100.000
     delay(3000);
     sendData(0xB7,0x00000000); // XTARGET = 0
     delay(3000);
    }
    
    void sendData(unsigned long address, unsigned long datagram)
    {
     //TMC4361 takes 40 bit data: 8 address and 32 data
    
     delay(100);
     unsigned long i_datagram;
    
     digitalWrite(chipCS,LOW);
     delayMicroseconds(10);
    
     SPI.transfer(address);
    
     i_datagram |= SPI.transfer((datagram >> 24) & 0xff);
     i_datagram <<= 8;
     i_datagram |= SPI.transfer((datagram >> 16) & 0xff);
     i_datagram <<= 8;
     i_datagram |= SPI.transfer((datagram >> 8) & 0xff);
     i_datagram <<= 8;
     i_datagram |= SPI.transfer((datagram) & 0xff);
     digitalWrite(chipCS,HIGH);
    
     Serial.print("Received: ");
     Serial.println(i_datagram,HEX);
     Serial.print(" from register: ");
     Serial.println(address,HEX);
    }
    

    Download

    The Arduino_TMC4361_TMC2130.zip file includes the pinning template, the TMC4361-EVAL schematic and the Arduino project.

    Related Pages

    • TMC4361A-LA
    • TMC4361A-LA datasheet download
    • TMC4361-EVAL
      (Please note: This EVAL version has the previous TMC4361-LA on board. This version is compatible as well. The TMC4361A-EVAL will be available shortly.)
    • TMC2130
    • TMC2130 datasheet download
    • TMC2130-EVAL

    Share this:

    • Twitter
    • Facebook
    • LinkedIn
    • Pinterest
    • Print
    • Email

    Related

    June 27, 2017 / Lars Jaskulski / 28

    Categories: Products

    Tags: closed loop stepper

    How to drive a stepper motor with your Raspberry Pi 3/2 using a TMC5130-EVAL How to drive a stepper motor via UART with your Arduino Mega using a TMC5072-EVAL

    Comments are currently closed.

    28 thoughts on “How to drive a stepper motor closed loop with your Arduino Uno using a TMC4361A-EVAL + TMC2130-EVAL”

    • Sam says:
      September 28, 2017 at 8:25 pm

      How can i read register value through SPI

    • Lars Jaskulski says:
      November 21, 2017 at 12:26 pm

      Hi,

      Please check the datasheet chapter 3. SPI Interfacing.
      All registers that are readable simply need to be send with its corresponding address with any dummy data. An example can be found in the datasheet on page 17 (datasheet revision 1.21).

      Best regards,
      Lars

    • ZANG says:
      November 24, 2017 at 11:15 am

      Do you have the sample code for Step/Dir mode to control the stepper motor (TMC4361A-LA+ TMC2130), THANKS..

    • Lars Jaskulski says:
      November 24, 2017 at 11:31 am

      Hi Zang,

      Yes, please download this file: http://blog.trinamic.com/wp-content/uploads/2017/11/TMC2130_closedLoop_sd.zip

      This is a .tpc file. If you are using the Landungsbrücke or Startrampe you can use our TMCL IDE to start the PC/Host host Tool you’ll find in the menu entry Tools.
      Within the Tool click on the TMCL/PC menu entry and select Options… –> Module Assignments….
      Now you need to assign the listed module on the left to the Assigned modules on the right, cope to code and edit the first line of the above .tpc file with the generated line.

      If you do not use the Landungsbrücke or Startrampe you can use the .tpc file to help yourself on editing the code. All WMCcommands mean write to micro controller. Those are essentially the SPI commands.

      I hope this helps you. Let us know if you need further assistance!

      Best regards,
      Lars

    • ZANG says:
      November 27, 2017 at 4:02 am

      Thanks, I will try it and come back to you..

    • Zang says:
      November 27, 2017 at 4:15 am

      Hi, Lars Jaskulski

      I have set up the platform with my own micro controller, and test the source code with above, but now the problem is that I need the similar source code for Step/Dir mode to verify the chip & driver performance. So right now maybe it’s not possible to get the hardware Landungsbrücke or Startrampe from trinamic.

      Is it possible to send me the text file similar to the code above to my email.
      jian.zang1126@gmail.com

      Thanks.

      Regards
      Zang Jian

    • Zang says:
      November 27, 2017 at 5:36 am

      Hi, Lars Jaskulski

      I have opened the file, will try it.. Thanks.

      Regards
      Zang Jian

    • Lars Jaskulski says:
      November 27, 2017 at 11:33 am

      Hi Zang Jian,

      Let me know if and how it worked for you. If anyhting is unclear or you need additional help, please do not hesitate to reply on this comment.

      Regards,
      Lars

    • zang says:
      November 28, 2017 at 4:42 am

      HI, Lars Jaskulski

      So far so good. I try to summarize some questions and list on this comments together later..

      Thanks for your reply..

      Regards
      Zang Jian

    • Zang says:
      November 29, 2017 at 8:47 am

      Hi, Lars Jaskulski

      I have summarized 5 questions below. Hopefully you can help me on this..

      1. Under SPI mode Max speed.
      Based on my hardware:
      External clock: 16Mhz
      Encoder resolution: 500 cpr
      Stepper motor : 200 step per revolution
      a. How to calculate the max speed limit VMAX in pps?
      b. Based on the sample code (SPI mode),
      sendData(0xA4,0x079A8000); // VMAX = 400.000 pps (equals to 120 rpm?)
      079A8000 equals 489304.00000000
      This value how to match 400.000pps. What’s the formula?
      c. When I test the speed with my existing hardware (described as above), I measure the actual motor speed is around 570 rpm, not same to the sample 120 rpm (400.000 pps).
      Is it any hardware or parameter I need to set differently due to the hardware difference?
      2. Motor engaged status
      During my testing stage, I noticed that the moto engaged status (when the motor shaft become engaged) is related to 2 registers below.
      0x07 0x00400000 & 0x54 0x00009C40
      But how to get back the motor engaged/disengaged status? Can you show me the register?
      3. The last question is I want to confirm is that from the SPI mode sample code: the encoder resolution 10.000cpr
      This encoder is 10cpr or 10,000cpr?
      I guess it is supposed to be 10cpr, but its resolution seems too low, and this type of encoder is very rare, so a bit unbelievable.
      I am a beginner with the Trinamic Chip, if whatever statement I made is not clear or wrong , please let me know.. Thanks.

      If any information you want to know about my platform, just let me know.
      Thanks a lot..

      Regards
      Zang Jian

    • Peca says:
      January 13, 2018 at 8:20 pm

      Hi, Lars Jaskulski!
      Am I missing something, but can`t find the type and model of the incremental encoder… I am trying but only text that I find is: “The encoder used for this test has a resolution of 10.000 cpr respective a resolution of 40.000.”.
      And if it is possible, explain to me (us) the difference between the two data: 10x10e3 cpr and 40x10e3.
      Thank you.
      Regards,
      Predrag Pesic.

    • Dusit says:
      January 17, 2018 at 8:45 am

      When load increases Will speed remain?

    • Lars Jaskulski says:
      January 22, 2018 at 4:34 pm

      Hi Dusit,

      Yes. First the controller will adapt automatically the current to overcome the load condition by maintaining the target velocity. If the physics of the motor are exceeded, with more load than the motor can handle with set maximum current, the motor will reduce velocity as long as the overload condition happens. As soon as the overload condition is overcome, the motor will speed up to catch up the “lost” time.

      Hope that answers your question sufficient. Let us know if you need further assistance.

      Best regards,
      Lars

    • Lars Jaskulski says:
      January 22, 2018 at 4:44 pm

      Hello Predrag,

      The used encoder is one of our own: https://www.trinamic.com/products/drives/encoder-details/tmcs-28/

      The encoder has 10.000 lines but as the A and B signal are 90 degrees out of phase you have in total 40.000 “states”. This is called quadrature.

      Please let us know if this answer is sufficient and if we can further assist you.

      With best regards,
      Lars

    • J says:
      June 8, 2018 at 10:12 am

      Hey

      Thanks for this cool Projekt.

      I done the Projekt without an Encoder. Was a bit difficult to change the code (not really good in software )

      But it worked fine.

      Best regards

      J

      PS: This blog is really nice!

    • Lars Jaskulski says:
      June 20, 2018 at 9:22 am

      Thanks J for the positive feedback!
      Best regards,
      Lars

    • Thomas says:
      October 21, 2018 at 5:58 pm

      Hi Lars

      Thanks for this inspiring article! I’m currently working on a pan/tilt controller with the same hardware, but with open loop. Adapting your code I was able to get the motor oscillating the two revolutions forth and back, however with a terrible high frequency noise. I figured out that using Timer1 on an Arduino Uno gives you only 8MHz, so the whole system (including PWM) runs on half of the speed.

      If somebody is using an ATmega32U4, there’s an easy workaround by using Timer4 with USB’s 96MHz PLL clock input. For my setup with port B5 (/OC4B), this code sets up the required 16MHz signal:

      PLLFRQ |= _BV(PLLTM0); // use 96MHz PLL clock for Timer 4
      TCCR4B = _BV(CS40) + _BV(PWM4X); // PCK/1 in asynchronous mode
      TCCR4D = 0; // WGM41/40 = 0, Fast PWM, OCR4C = TOP
      OCR4C = 0x5; // TOP value: 96MHz / 6 = 16MHz
      TCCR4A = _BV(COM4B0) // /OC4B connected, cleared on compare match, set when TCNT4 = 0,
      + _BV(PWM4B); // enable PWM based on OCR4B
      OCR4B = 2; // 50% duty cycle

      Best regards,
      Thomas

    • Conner Currier says:
      November 14, 2018 at 1:47 am

      Hi. How can I wire the TMC2130 directly to an Arduino Uno, without using the EVAL board? Thanks!

    • Lars Jaskulski says:
      November 14, 2018 at 9:16 am

      Hi Conner,

      Please follow this link: http://blog.trinamic.com/2017/04/05/how-to-use-tmc5130-eval-with-your-arduino-mega/

      The connection is pretty much the same with the TMC2130-EVAL except you need to connect the STEP/DIR signals of course.

      Hope that guides you to the right direction.

      Best regards,
      Lars

    • Lars Jaskulski says:
      November 30, 2018 at 11:53 am

      Hi Thomas,

      Sounds great. Have you tried out stealthChop already? This will make the motor completely silent and helps with smoothness in motion as well.

      You can activate it by setting these values to the PWMCONF (0x70) register:

      PWM_AMPL = 200
      PWM_GRAD = 4
      pwm_freq = 2 or 3 with 8MHz CLK
      pwm_autoscale = 1

      To activate stealthChop set bit en_pwm_mode to 1 in the GCONF register (0x00).

      Hope that makes your motor smooth and audbibly noiseless.

      Best regards,
      Lars

    • Amer says:
      December 12, 2018 at 4:17 am

      Hi Lars,
      i am having hard time getting the above codes working properly. the stepper motor only does one step. would you mind sending a simple code that makes the stepper motor rotates in one direction with steady velocity.

      Best regards
      Amer

    • Kevin says:
      December 31, 2018 at 7:33 am

      How about an Arduino + TMC4361A-BOB + TMC2660-BOB ? Any compatibility issues?
      Thanks

    • Lars Jaskulski says:
      January 3, 2019 at 2:23 pm

      Hi Kevin,

      With the proper connections it would pretty much act the same way as the EVAL boards so no issues. As you get the schematics on the respective pages on our webpage should be an easy translation.

      Best regards,
      Lars

    • Lars Jaskulski says:
      January 3, 2019 at 2:25 pm

      Hi Amer,

      I assume that the encoder you are using might have another resolution as in this example? Have you changed those settings already? The encoder resolution needs to match the encoder you are using and you need to make sure the direction of the encoder is matching the motor direction. If not you can change the cables to have it matched or change the direction bit in the TMC4361A.

      Best regards,
      Lars

    • Andrew Chin says:
      February 7, 2019 at 5:35 pm

      To confirm, the external clock on the TMC4361 is being controlled by the timer pin (pin 9) on the arduino @ a rate of 16 Mhz? What is the minimum ans max speed we can run the clock at on the TMC4361?

    • Lars Jaskulski says:
      February 8, 2019 at 11:23 am

      Hi Andrew,

      Yes, pin 9 is been used with a timer and toggles the pin with every system CLK cycle. Therefore the CLK output respectively CLK of the TMC436A-LA is 8MHz.

      The minimum is 4.2MHz and the maximum is 30MHz. Please have a look into the TMC4361A-LA datasheet Rev. 1.24 on page 219.

      Best regards,
      Lars

    • Lorenzo Leandro says:
      January 13, 2020 at 11:38 am

      I have connected everything as in the post and I am using a 12V power supply on the TMC2130.
      The motor makes a high picked noise but does not move.
      I would like to measure speed while moving at ~1round per second.
      Do you have any suggestions?

      Thanks in advance,
      Lorenzo

    • Lars Jaskulski says:
      January 13, 2020 at 11:43 am

      Hi Lorenzo,

      My first guess is that the encoder is not set up properly. Have you checked encoder wiring, encoder resolution and set up the registers respectively in the TMC4361?

      Best regards,
      Lars

Recent Posts

  • Guest Blog: UBC Thunderbots
  • Guest Blog: TURAG
  • Guest Blog: School of Engineering Telecom Physics University of Strasbourg
  • Guest Blog: STAR Dresden
  • Pushing the Limits of Stepper Motor Control in 3D Printing

Social

  • View trinamic.mc’s profile on Facebook
  • View Trinamic_MC’s profile on Twitter
  • View trinamic’s profile on GitHub
  • View UC4SHA5_GAw1Wbm2T2NWpWbA’s profile on YouTube

Meta

  • Log in
  • Entries feed
  • Comments feed
  • WordPress.org

Newsletter

 

Info

Waterloohain 5
22769 Hamburg
+49 40 514 806 40

Tags

3D printer 3d printing arduino BLDC cDriver code code snippet coolStep drive Driver ease of use ethercat eval kit evaluation kit Field Oriented Control FOC GUI guide heliostat How-To IC linear stage motion controller motor driver open source hardware programming rtmi servo servo controller IC setup silentstepstick stealthChop stepper stepper motor stepper motor driver stepper motor driver ic stepper motors technology TMC4671 tmc5130 TMCL TMCL-IDE TRINAMIC TRINAMIC Motion Control tuning

Tag Cloud

    3D printer 3d printing arduino BLDC cDriver code code snippet coolStep drive Driver ease of use ethercat eval kit evaluation kit Field Oriented Control FOC GUI guide heliostat How-To IC linear stage motion controller motor driver open source hardware programming rtmi servo servo controller IC setup silentstepstick stealthChop stepper stepper motor stepper motor driver stepper motor driver ic stepper motors technology TMC4671 tmc5130 TMCL TMCL-IDE TRINAMIC TRINAMIC Motion Control tuning

Pages

  • Blog Ethics
  • Newsletter
  • Player Embed
  • Search Videos
  • User Videos
  • Video Category
  • Video Tag

Categories

  • Competition
  • Guest blog
  • Industry News
  • Jobs
  • Myths about Motors
  • Open Source Hardware
  • Products
  • Projects
    • DIY
    • University Projects
  • Research
  • Social
  • Software
  • technology
  • Trade Shows
  • Tutorial
    • Code Snippet
  • Uncategorized

Copyright © 2015 TRINAMIC BlogTheme created by PWT. Powered by WordPress.org

loading Cancel
Post was not sent - check your email addresses!
Email check failed, please try again
Sorry, your blog cannot share posts by email.