( L6470.fth )

TACHYON

pub L6470.fth ." L6470 MicroStepper V1.2 130604.1300" ;

{

L6470 dSPIN fully integrated microstepping motor driver with motion engine and SPI

Features

■ Operating voltage: 8 - 45 V

■ 7.0 A output peak current (3.0 A r.m.s.)

■ Low RDSon power MOSFETS

■ Programmable speed profile and positioning

■ Programmable power MOS slew-rate

■ Up to 1/128 microstepping

■ Sensorless stall detection

■ SPI interface

■ Low quiescent and standby currents

■ Programmable non dissipative overcurrent protection on high and low-side

■ Two levels overtemperature protection

■ HTSSOP28 package

Website for L6470 IC

PSM6470 module with L6470

VIDEO

Implemented on PuppySolo - working configuraton uses two L6470 modules

#busy and #flag lines are monitored but are superfluous as status register can be read

Step clock STCK lines could be useful for CNC modes where steps have to be synchronized

#rst is probably not needed either but is a quick way to reset all stepper drivers.

P19 & P20 are connected to optional on-board LEDs but otherwise are used for the SD card & a MOSFET drive

Ported to Tachyon Forth 120823

V1.2 has enhanced SPI timing

Code stats:

CODE @$3DF6 - bytes used in this load = 1918

NAMES @$1C83 - bytes used in this load = 903

CHANGELOG:

130604 - Changed REV and FWD to REVERSE and FORWARD to avoid conflict with native instruction REV

121002 - Fixed and updated support for step clock mode

120919 - Removed reference to slave or master in !L6470 so each can be defined separately

}

\ Define port constants if not already defined

IFNDEF #miso

#P0 |< CONSTANT #miso \ MISO data from L6470 - multiple outputs are commoned with 220R in each output.

#P1 |< CONSTANT #busy \ busy flag - pullup with 10K

#P2 |< CONSTANT #master \ Select master chip

#P3 |< CONSTANT #slave \ slave chip (or daisy chained)

#P4 |< CONSTANT #stepmaster \ step clock (optional, not used)

#P5 |< CONSTANT #stepslave

#P6 |< CONSTANT #flag \ pullup with 10K

#P7 |< CONSTANT #rst \ Chip standby/reset - pulled down with 10K

#P28 |< CONSTANT #sck \ SCL is shared with SCK

#P29 |< CONSTANT #mosi \ SDA is shared with MOSI

#P19 |< CONSTANT #ambled

#P20 |< CONSTANT #redled

#P2 2 MASKS CONSTANT CSPINS

}

LONG response,unit,status,stepclk

--- Select which chip will be accessed

pub SELECT ( pinmask -- ) unit ! ;

pub CS unit @ ;

pub MASTER #master SELECT #stepmaster stepclk ! ;

pub SLAVE #slave SELECT #stepslave stepclk ! ;

pub !SPIO \ Initializse SPIO module etc

[SPIO] \ Load SPIO module

0 $1E8 COG! \ disable SPIO module instruction which returns clock low

8 #miso #mosi #sck COGREGS \ setup masks to be used by SPIO module

;

pub TXBITS ( cnt -- )

@CNT COGREG! \ set number of bits to use for SPIO

;

pub SPI \ Execute an SPI operation

#mosi OUTCLR RUNMOD \ Make sure the data line is an output in case it is shared with SDA

;

pub TX

#redled OUTSET

BEGIN #busy IN UNTIL \ Wait until the chip is ready

#redled OUTCLR

pub TXRAW ( data -- ) \ <40us execution time

#ambled OUTSET

CSPINS OUTSET

CS OUTCLR

#24 SHL SPI \ runs SPIO at 2.8MHz

CS OUTSET

response @ 8 SHL OR response !

#ambled OUTCLR

;

\ Transmit the command and the data as four 8-bit packets with CS deasserted between packets

pub TXCMD ( data cmd -- )

#24 SHL OR

#ambled OUTSET

CSPINS OUTSET

4 FOR

CS OUTCLR

SPI

CS OUTSET

response !

#ambled OUTCLR

;

\ Direction is a VARIABLE to be added to the command so that it can

\ be "set and forget", no need to specify each time.

BYTE dir --- create a variable to remember the last selected direction

pub FORWARD 1 dir C! ;

pub REVERSE dir C~ ;

pri +DIR dir C@ OR ; \ adds direction into the command

DECIMAL

\ this little table holds the bit size of each register so that read and writes of longs can be automatically formatted

TABLE regsize

00 | 22 | 09 | 22 |

20 | 12 | 12 | 10 |

13 | 08 | 08 | 08 |

08 | 14 | 08 | 08 |

08 | 04 | 05 | 04 |

07 | 10 | 08 | 08 |

16 | 16 | 00 | 00 |

{

Writing to a register is a SetParam command with the register address as the PARAM field

Up to 3 bytes of data are needed but unfortunately the byte order varies depending upon the register's size

So a lookup is performed in the regsize table and the data arranged accordingly

}

pub REG! ( dat reg -- )

response ~

DUP $1F AND regsize + C@ \ determine bit size of the register ( dat reg size )

DUP 9 <

IF DROP

ELSE \ 8 bits or less ? ( dat reg )

$10 >

IF TXRAW DUP $10 SHR THEN \ 16 bits or more ? THEN send: reg bits16..24

TXRAW DUP 8 SHR \ 9 to 15 bits sendpub cmd bits8..15

THEN

TXRAW TXRAW \ this is either CMD+DAT or trailing

;

\ Modify register address into a GetParam command and send null data while the TXRAW routine reads the response

pub REG@ ( reg -- data )

0 SWAP $20 OR REG! \ issue corresponding bytes

response @ \ pickup response assembled by TXRAW

;

\ Getstatus does not wait if the L6470 is busy but performs a non-invasive status read

pub GetStatus ( -- status ) \ if no response is received (no chip) THEN return with 0 as status

0 $D0 TXCMD

response @ 8 SHR

DUP $FFFFFF = IF DROP 0 THEN

;

pub CMD ( dat cmd -- ) \ 60us with 100us cycle

TXCMD

GetStatus DROP

;

\ L6470 application commands - named similar as per programming manual

pub Nop \ also useful to cause application to wait until controller is ready

0 TX

;

pub Run ( speed -- )

$50 +DIR CMD

;

pub MOVE ( steps -- )

$40 +DIR CMD

;

pub GoTo ( position -- ) \ Go to an absolute position

$60 CMD

;

pub GoToDIR ( position -- )

$68 +DIR CMD

;

pub GoUntil ( speed act -- )

3 SHL $82 OR +DIR CMD

;

pub ReleaseSW ( act -- )

3 SHL $92 OR +DIR CMD

;

pub GoHome $70 TX ;

pub GoMark $78 TX ;

pub ResetPos $D8 TX ;

pub ResetDevice $C0 TX ;

pub SoftStop $B0 TX ; \ Stop after deceleration

pub HardStop $B8 TX ; \ Stop immediately

pub SoftHiZ $A0 TX ; \ Decelerate before bridge disconnection

pub HardHiZ $A8 TX ; \ Immediate bridge disconnection

\ Enter step clock mode

pub STEPCK

$58 +DIR TX

;

pub XSTEP

stepclk @ DUP OUTSET OUTCLR

;

pub XSTEPS ( cnt dly -- )

SWAP FOR XSTEP DUP FOR NEXT NEXT DROP

;

\ Register names - mainly for convenience, diagnostics, and readability (also used for listing registers)

$01 CONSTANT @POS

$02 CONSTANT @EPOS

$03 CONSTANT @MARK

$04 CONSTANT @SPEED \ current speed

$05 CONSTANT @ACC \ acceleration

$06 CONSTANT @DEC \ deceleration

$07 CONSTANT @MAX \ max speed

$08 CONSTANT @MIN \ min speed

$09 CONSTANT @HOLD \ hold current

$0A CONSTANT @RUN \ run current * adjust this for slower speeds - while running

$0B CONSTANT @STACC \ accel starting current

$0C CONSTANT @STDEC \ dec staring current

$0D CONSTANT @INTSPD \ Intersect speed

$0E CONSTANT @STSLP \ start slope - BEMF compensation curve

$0F CONSTANT @ACCFS \ Acceleration final slope

$10 CONSTANT @DECFS \ Deceleration final slope

$11 CONSTANT @THERM \ Thermal compensation factor

$12 CONSTANT @ADC \ 5 bits of ADC reading supply voltage

$13 CONSTANT @OCD \ overcurrent threshold

$14 CONSTANT @STALL \ stall threshold

$15 CONSTANT @FSSPD \ full step speed threshold

$16 CONSTANT @STEPMD \ controls number of microsteps or none

$17 CONSTANT @ALARMS \ alarm mask

$18 CONSTANT @CONFIG \ PWM and clock control etc

$19 CONSTANT @STATUS

$00 CONSTANT @REGS

DECIMAL

\ Initialization table values to suit motor and application

\ Multiple tables can be setup for various motors and the table pointer

\ passed to the INIT routine if necessary. Just this one at present.

\ This is a 34HY1801 Nema34

TABLE inits \ word aligned structure

\ some motor properties in same table but not part of L6470 registers

\ first 5 values are not loaded into registers but represent motor CONSTANTs

200 || \ steps/rev

13334 || \ rpmmul

60 || \ rpmdiv

0 ||

\ REG 5

600 || \ @ACC Acceleration

400 || \ @DEC Deceleration

452 || \ @MAX Maximum speed (for sm7)

0 || \ @MIN Minimum

20 || \ @HOLD Holding current

50 || \ @RUN Run current

100 || \ @STACC Acceleration starting current

32 || \ @STDEC Deceleration starting current

1032 || \ @INTSPD Intersect speed

80 || \ @STSLP Start slope

40 || \ @ACCFS Acceleration final slope

\ REG $10

64 || \ @DECFS Deceleration final slope

0 || \ @THERM Thermal compensation

0 || \ @ADC ADC (read only)

15 || \ @OCD Over-current threshold

127 || \ @STALL Stall Threshold (set to max)

550 || \ @FSSPD Full step speed (changes from microstep)

7 || \ @STEPMD Step mode

$FF || \ @ALARMS Alarm enables

\ REG $18

$1EA0 || \ @CONFIG Config ( 2E88 )

0 || 0 || 0 || \ reserved

LONG @inits

pub INIT@ 2* @inits @ + W@ ;

\ *** Some console friendly aliases **

\ Stop and also read the status to clear any errors

pub STOPMTR SoftStop GetStatus DROP ;

\ Stop immediately, override any operation

pub HALT $B8 TXRAW ;

\ Disconnect the bridge - also needed before certain commands can be written

pub DISC SoftHiZ ;

pub HOME GoHome ;

pub GO GoTo ;

pub CONFIG! STOPMTR DISC @CONFIG REG! ;

pub PWMDIV ( 1..7 --)

1- 0 MAX 7 AND 6 MIN $0D SHL @CONFIG REG@ $E000 ANDN OR CONFIG!

;

pub PWMDEC ( 0..7 -- ; multiply the PWM freq from 0.625 to 2 in 0.125 increments )

7 AND #10 SHL @CONFIG REG@ $1C00 ANDN OR CONFIG!

;

pub SLEW ( 0..3 -- ; adjust the slew rate as 180,180,290,530 )

3 AND #16 SHL @CONFIG REG@ $300 ANDN OR CONFIG!

;

pub VSCOMP ( on/off -- )

$20 AND @CONFIG REG@ $20 ANDN OR CONFIG!

;

pub LSCOMP ( rpm -- ) \ 300RPM = 4194

#292 MIN STOPMTR #8388 * #600 / #4096 + @MIN REG!

;

pub !REGS ( inittbl -- )

@inits !

HALT DISC $19 5 DO I INIT@ I REG! LOOP

;

pub !L6470IO

!SPIO

#rst OUTCLR #rst OUTSET \ Hardware reset - pulse line low

#sck OUTCLR #sck OUTSET #mosi OUTSET \ Use I2C lines as outputs for clock and data in (L6470)

#slave OUTSET

#master OUTSET

#stepmaster OUTSET

#stepslave OUTSET

#redled OUTSET

#ambled OUTSET

FORWARD

;

\ Initialize the L6470 stepper chip using the table supplied ( or default = 0)

pub !L6470 ( tbl -- )

0 COGREG@ #sck <> IF !L6470IO THEN

DUP 0= IF DROP inits THEN

!REGS GetStatus status W!

;

pub STEPS ( steps -- )

@STEPMD REG@ SHL $3FFFFF MIN MOVE

;

pub TURNS ( turns -- )

0 INIT@ * STEPS

;

pub READY? ( -- rdyflg )

GetStatus 2 AND

;

pub BUSY? ( -- bsyflg )

READY? 0=

;

\ Run motor at speed to match rpms

pub RPM ( rpm -- )

?DUP IF #13334 * #60 / Run ELSE STOPMTR THEN

;

\ Rough conversion from ADC reading to voltage (pretty close)

pub VOLTS ( -- volts )

@ADC REG@ 9 * 3 SHR

;

pri BIT? DUP 0 REG @ AND ... 0 REG DUP @ 2/ SWAP ! ;

pub ShowStatus \ Simply for console diagnostics to see the status register conditions

DUP IF

$8000 0 REG !

BIT? IF CR ." Step Clock Mode" THEN

BIT? 0= IF CR ." Step Loss B" THEN

BIT? 0= IF CR ." Step Loss A" THEN

BIT? 0= IF CR ." Over Current" THEN

BIT? 0= IF CR ." Thermal Shutdown" THEN

BIT? 0= IF CR ." Thermal Warning" THEN

BIT? 0= IF CR ." Under voltage" THEN

BIT? IF CR ." Wrong command" THEN

BIT? IF CR ." Command not performed" THEN

CR ." Speed = " @SPEED REG@ 2 INIT@ * 1 INIT@ / . ." RPM"

DUP $60 AND 5 SHR

CR ." Motor = "

DUP 0= IF ." stopped" THEN

DUP 1 = IF ." accelerating" THEN

DUP 2 = IF ." decelerating" THEN

3 = IF ." constant" THEN

BIT? DROP BIT? DROP

BIT? IF ." forward" ELSE ." reverse" THEN

BIT? IF CR ." Switch Event" THEN

BIT? IF CR ." Switch Closed" THEN

BIT? 0= IF CR ." Busy" THEN

BIT? IF CR ." Bridge disconnected" THEN

CR ." Steps/full step = " @STEPMD REG@ 1 SWAP SHL #10 .NUM

CR ." Input Voltage = " VOLTS #10 .NUM ." V"

ELSE

." - No device "

THEN

DROP

;

\ ********************* DEMO & DEBUG **********************

\ Short diagnostic functions which simplify keyboard entry

\ List Registers ( in current number base )

\ Use the name for the register as provided in the compiled CONSTANT names

NFA' @REGS 1+ CONSTANT regstr

pri .NAME ( index -- )

$1A SWAP - regstr SWAP FOR BEGIN C@++ $80 AND UNTIL 2+ NEXT

1+ DUP .STR #10 SWAP STRLEN - SPACES

;

\ List names and contents of registers of both MASTER and SLAVE as well as their default values

pub LR

CR $470A 8 REG W!

." REG NAME SZ MASTER SLAVE DEFAULT "

\ 01pub 0020571A 00000000 000000E7

$1A 1 DO

I $16 > IF $4810 8 REG W! THEN

CR I .BYTE ":" EMIT SPACE I .NAME

regsize I + C@ $20A .NUM 2 SPACES

MASTER I REG@ 8 REG W@ .NUM 3 SPACES SLAVE I REG@ 8 REG W@ .NUM

4 SPACES I INIT@ 8 REG W@ .NUM

LOOP

CR MASTER

;

{ Format of LR (list registers)

REG NAME MASTER SLAVE DEFAULT

01: @POS 0,000,000 0,000,000 0,013,334

02: @EPOS 0,000,000 0,000,000 0,000,060

03: @MARK 0,000,000 0,000,000 0,000,000

04: @SPEED 0,000,000 0,000,000 0,000,000

05: @ACC 0,000,100 0,000,120 0,000,120

06: @DEC 0,000,400 0,000,400 0,000,400

07: @MAX 0,000,300 0,000,300 0,000,300

08: @MIN 0,000,000 0,000,000 0,000,000

09: @HOLD 0,000,020 0,000,020 0,000,020

0A: @RUN 0,000,250 0,000,040 0,000,040

0B: @STACC 0,000,120 0,000,070 0,000,070

0C: @STDEC 0,000,080 0,000,080 0,000,080

0D: @INTSPD 0,001,032 0,001,032 0,001,032

0E: @STSLP 0,000,200 0,000,080 0,000,080

0F: @ACCFS 0,000,120 0,000,040 0,000,040

10: @DECFS 0,000,064 0,000,064 0,000,064

11: @THERM 0,000,000 0,000,000 0,000,000

12: @ADC 0,000,020 0,000,021 0,000,000

13: @OCD 0,000,015 0,000,015 0,000,015

14: @STALL 0,000,127 0,000,127 0,000,127

15: @FSSPD 0,000,550 0,000,550 0,000,550

16: @STEPMD 0,000,007 0,000,007 0,000,007

17: @ALARMS 0000_00FF 0000_00FF 0000_00FF

18: @CONFIG 0000_1EA0 0000_1EA0 0000_1EA0

19: @STATUS 0000_7E03 0000_7E03 0000_0000

}

\ List the status of the chips

pub LS \ List the status of the chip(s) for diagnostics

\ busy and flag pins are common

P@ DUP 2 AND 0= IF ." =busy " THEN $40 AND 0= IF ." =flag " THEN

SLAVE GetStatus DUP IF CR ." --------SLAVE-------- " DUP ShowStatus THEN DROP

CR MASTER GetStatus DUP IF CR ." --------MASTER-------- " DUP ShowStatus THEN DROP

CR ;

{ Format of LS (list status)

--------SLAVE--------

Motor = stopped reverse

Steps/full step = 128

Input Voltage = 28V

--------MASTER--------

Motor = constant forward

Steps/full step = 128

Input Voltage = 28V

}

\ Stop both motors

pub S

CS SLAVE STOPMTR MASTER STOPMTR SELECT

;

\ set step mode 0..7 (up the max on full step)

pub SM ( mode -- )

DISC DUP @STEPMD REG!

IF $1C0 ELSE $2FF THEN

@MAX REG!

;

\ Set RUN current as in: 30 RC

pub RC ( current -- )

@RUN REG!

;

HEX

END

MASTER

BACKUP