TACHYON

pub L6470  ." L6470 MicroStepper V1.1 120823" ;

{
  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
}


0	MASK CONSTANT .SDI	'' MISO data from L6470 - multiple outputs are commoned with 220R in each output.
1	MASK CONSTANT .BUSY	'' busy flag - pullup with 10K 
2	MASK CONSTANT .CS1	'' Select master chip
3	MASK CONSTANT .CS2	'' slave chip (or daisy chained)
4	MASK CONSTANT .STCK1	'' step clock (optional, not used)
5	MASK CONSTANT .STCK2
6	MASK CONSTANT .FLAG	'' pullup with 10K
7	MASK CONSTANT .RST	'' Chip standby/reset - pulled down with 10K

#28 	MASK CONSTANT .SCL	'' SCL is shared with SCK
#29	MASK CONSTANT .SDA	'' SDA is shared with MOSI

#19	MASK CONSTANT .AMBLED	
#20	MASK CONSTANT .REDLED


	VARIABLE response
	VARIABLE unit


pub SELECT	unit ! ;
pub CS		unit @ ;
pub MASTER	.CS1 SELECT ;
pub SLAVE	.CS2 SELECT ;


pub TXRAW   '' <40us execution time   
	.AMBLED OUTSET
	.SCL OUTSET
	CS OUTCLR
	#24 SHL RUNMOD response !		'' runs SPIO at 2.8MHz
	CS OUTCLR
	.AMBLED OUTCLR
	;

pub WAIT	 .REDLED OUTSET BEGIN .BUSY IN UNTIL .REDLED OUTCLR ;

pub TX	 WAIT TXRAW ;

'' Made direction a VARIABLE to be added to the command so that it could
'' be "set and forget", no need to specify each time.

	CVARIABLE dir
	
pub FWD   	1 dir C! ;
pub REV		0 dir C! ;
pub +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 sendpub reg bits16..24
	  TXRAW DUP 8 SHR THEN          '' 9 to 15 bits sendpub cmd bits8..15
	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
	$D0 TXRAW 0 TXRAW 0 TXRAW 0 TXRAW response @ 8 SHR DUP $FFFFFF = IF DROP 0 THEN ;



pub CMD  ( dat cmd -- )
	TX DUP $10 SHR TX DUP 8 SHR TX $FF AND TX
	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 STEPCK
	$58 +DIR TX
	;
pub XSTEP
	.STCK1 DUP OUTSET OUTCLR
	;
pub XSTEPS ( cnt dly -- )
	SWAP FOR XSTEP DUP FOR NEXT NEXT DROP
	;
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


'' Register names - mainly for convenience and readability
'' Keep these names to less than 8 characters to maintain an indexed NFA
$00 CONSTANT @REGS
$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 @DECDS	'' 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

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	||
	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
'' 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
'' 18
	$1EA0	||	'' @CONFIG Config ( 2E88 )
	0 || 0 || 0 ||	'' reserved

	VARIABLE @inits

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


'' *** Some console friendly aliases **

'' Stop and also read the status to clear any errors
pub STOP		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!		STOP 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 -- )
	IF $20 ELSE 0 THEN @CONFIG REG@ $20 ANDN OR CONFIG!
	;

pub LSCOMP ( rpm -- )     '' 300RPM = 4194
	  #292 MIN STOP #8388 * #600 / #4096 + @MIN REG!
	  ;

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

	VARIABLE status

pub !L6470  ( tbl -- )     '' Initialize the L6470 stepper chip using the table supplied ( or default = 0)
	DUP 0= IF DROP inits THEN @inits !
	[SPIO] 8 3 COGREG! .SCL 0 COGREG! .SDA 1 COGREG! .SDA 2 COGREG!
	.RST OUTCLR .RST OUTSET			'' Hardware reset - pulse line low
	.SCL OUTCLR .SCL OUTSET .SDA OUTSET		'' Use I2C lines as outputs for clock and data in (L6470)
	.CS2 OUTSET
	.CS1 OUTSET
	.STCK1 OUTSET
	.STCK2 OUTSET
	.REDLED OUTSET
	.AMBLED OUTSET
	SLAVE !REGS GetStatus status 2+ W!
	MASTER !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=
	;

pub RPM ( rpm -- ) '' Run motor at speed to match rpms
	#13334 * #60 / RUN
	;

'' Rough conversion from ADC reading to voltage (pretty close)
pub VOLTS    
	@ADC REG@ 9 * 3 SHR 
	;

	VARIABLE bit

pri BIT?
	DUP bit @ AND
	bit @ 2/ bit !
	;


pub ShowStatus		'' Simply for console diagnostics to see the status register conditions
	DUP IF 
	  $8000 bit !
	  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
	  DUP 0=	IF ."  stopped" THEN
	  DUP 1 =	IF ."  accelerating" THEN
	  DUP 2 =	IF ."  decelerating" THEN
	  3 =		IF ."  CONSTANT" THEN

	  '' 10 bit L!
	  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 .
			   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
' @POS CONSTANT regstr
pub .NAME  	DROP ;


'' List names and contents of registers of both MASTER and SLAVE as well as their default values
pub LR
	." REG NAME      MASTER   SLAVE       DEFAULT "
	''  01pub          0020571A 00000000    000000E7
	$1A 1 DO 
	  CR I .BYTE ":" EMIT SPACE I .NAME
	  MASTER I REG@ .LONG SPACE SLAVE I REG@ .LONG
	  4 SPACES I INIT@ .LONG 
	LOOP 
	CR MASTER
	;


'' List the status of the chips
pub L	'' 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  ;

'' Stop both motors
pub S	
	CS SLAVE STOP MASTER STOP 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!
	;

'' write config register as inpub   2EA8 CFG!
pub CFG! ( config -- )	
	DISC @CONFIG REG!
	;

'' Set RUN current as inpub   30 RC
pub RC ( current -- ) 
	@RUN REG!
	;	

END