// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *  
// BV4213 Motor controller
// This has an I2c interface and also a step and direction 
// interface. When using the I2C interface it can also do PWM
// Capable of driving 4 dc motors 1 way, 2 DC motors fwd and rev
// or 1 stepper motor
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *  

// HISTORY:
// March 2008 * error with VER, fixed

// REQUIRES:
// #URL-lib "http://pin1.org/forthlib/flb/General/soft1.flb" 
// #URL-lib "http://pin1.org/forthlib/flb/General/pinsel.flb" 
// #URL-lib "http://pin1.org/forthlib/flb/I2C/i2c.flb"

// CONSTANTS:
integer device    // holds i2c address
// holds stepper parameters
integer steps
integer direction
integer speedval
integer hf
integer rampval

: i2s device s ; // all words begin with this
// --------- Stepper control --------------------------

: a20 i2s &20 sb p ; // start - continuous step
: a21 i2s &21 sb p ; // stop  
: a21a i2s &21 sb 1 sb p ; // stop with break  
: a22 ( steps -- ) i2s &22 sb dup &ff00 and 8 rshift sb &ff and sb p ;
// gets stepper parameters and puts them in the integer variables
: a24 ( -- [integers] ) i2s &24 sb device 1+ rs 
      rn 8 lshift rn +   // steps
      3 for rn next rl p
      => rampval => hf => speedval => direction => steps ;
: steps? a24 steps ; // # steps to go
// stores parameters
: a23 ( [integers] -- ) i2s &23 sb direction sb speedval  sb 
    hf sb rampval sb p ;
: dir ( 1|0 -- ) => direction a23 ; // set direction
: speed ( n -- ) => speedval a23 ;  // set speed
: h/f ( 0|1 -- ) => hf a23 ;       // set half or full step
: ramp ( n -- ) => 0 max 8 min rampval a23 ; // set ramp up 0-8

// ---------------- DC Motor --------------------------
: a1 ( 0|1 -- ) i2s 1 sb sb p ; // enable y1,y2
: a2 ( 0|1 -- ) i2s 2 sb sb p ; // enable y1,y2
: a3 ( 0|1 -- ) i2s 3 sb sb p ; // power off / on
: a4 ( 0-4 -- ) i2s 4 sb sb p ; // differential Y1,y2
: a5 ( 0-4 -- ) i2s 5 sb sb p ; // diferentail Y3,Y4
: a11 ( 0|1 -- ) i2s &11 sb sb p ; // Yn output
: a12 ( 0|1 -- ) i2s &12 sb sb p ; // Yn output
: a13 ( 0|1 -- ) i2s &13 sb sb p ; // Yn output
: a14 ( 0|1 -- ) i2s &14 sb sb p ; // Yn output
: a15 ( n -- ) i2s &15 sb sb p ; // pwm for channel a 0-255
: a16 ( n -- ) i2s &16 sb sb p ; // pwm for channel b 0-255

// -------------- Direct stepping ---------------------
// This is the direct stepping interface, the port connections
// used are as below. Note, this is a simple interface and so 
// there few words serve as an illustration only.
-18 constant dirs  // p1.18
-17 constant clk  // p1.17
-16 constant enable // p1.16

// set up all pins for o/p
: setup
  dirs io-out clk io-out enable io-out 
  0 enable p!   ; // and anable

: dirss ( 0|1 -- ) dirs p! ;
: t10 100 for 10 ms 0 clk p! 10 ms 1 clk p! next ;
   
// ============= System commands =========================

// prints firmware version
: ver 
   device s &90 sb 10 sb device 1+ rs  // read eeprom at 8
   cr ." Ver " rn . [char] . emit rl emit p
;    
// a55 will test if device is working and return t/f
: a55 ( -- t/f ) device s &55 sb device 1+ rs rn 1 = rn 2 = and rl 3 = and p ;

// read eeprom
// # number of bytes to read and 
// from is starting address of eeprom
: a90 ( #, from -- values ) 
  device s &90 sb sb device 1+ rs 
  1- for rn next 
  rl p ;
  
// write all of the byts that are on the stack to eerpm
// starting at address start. Note start has to be last value on stack
: a91 ( [ .. ] start -- )
  device s &91 sb sb
  depth for sb next
  p ;
  
: a94 ( -- ) device s &94 sb p ; // sleep
: a95 ( -- ) device s &95 sb p ; // reset
: a96 ( -- ) device s &96 sb p ; // restore factory defaults
// imediately sets new i2c address to eeprom
: a99 ( new -- ) device s &99 sb dup sb &55 sb &aa sb device sb => device p ;

// use this first to establish i2C address if device and
// to initialise i2c system with default speed, if sucess requirn -1
: 4213-init ( device -- -1|0 ) 
  => device   // set address 
  i2-init     // initialise host i2c
  a55         // check interface
  if
    a24       // fill parameter integers
    -1        // leave true
  else
    0         // leave false
  then  
;