/* File: MOTOR.C */

/****************************************************************************
*
* STK16XSW.MOTOR
* ==============
*
* Low level routines for stepping motor control
*
* TQ-Systems GmbH                                                         
* ---------------                                                         
* Customer: TQ-Components
* Project : STK16XSW
* Tools   : uVision 2.05
*
* Rev: Date:     Name:            Modification:
* ----+---------+----------------+------------------------------------------
* 100  16.01.01  A. Lichte        taken over from STK16X.506                
*****************************************************************************/

/****************************************************************************
*
*                            availability summary
*
* available for Starterkit: STK16X STK16XU
* conformed for Starterkit: STK16X STK16XU 
* available for Modul     : TQM164   TQM165   TQM165U  TQM166  TQM167
*                           TQM167UL TQM167U  TQM167LC
*                           
* conformed for Modul     : TQM164   TQM165   TQM165U  TQM166  TQM167
*                           TQM167UL TQM167U  TQM167LC
*****************************************************************************/

/*==========================================================================*
* pragmas:
*===========================================================================*/

/*=========================================================================*
* include files (#INCLUDE):
*==========================================================================*/
/*-------------------------------------------------------------------------*
* standard include files:
*--------------------------------------------------------------------------*/
#include <intrins.h>            /* intrinsic functions         */
#include <reg167.h>             /* special function register   */

/*-------------------------------------------------------------------------*
* project specific include files:
*--------------------------------------------------------------------------*/
#include "motor.h"              /* include motor defines       */
#include "timer.h"              /* include timer functions     */
#include "lcd.h"                /* include LC display function */
#include "i2cio.h"              /* include I2C bus functions   */ 

/*=========================================================================*
* modul internal definitions (#DEFINE):
*==========================================================================*/

/*=========================================================================*
* modul internal type definitions(TYPEDEF):
*==========================================================================*/

/*=========================================================================*
* modul internal constants (CONST):
*==========================================================================*/
const BYTE fstep_1[8] = {1,4,2,8,1,4,2,8};   /*full step mode, 1 winding  */
const BYTE fstep_2[8] = {5,6,10,9,5,6,10,9}; /*full step mode, 2 windings */
const BYTE hstep[8]   = {5,4,6,2,10,8,9,1};  /* half step mode */

/*=========================================================================*
* extern available constants (CONST):
*==========================================================================*/

/*=========================================================================*
* modul internal variables:
*==========================================================================*/

/*-------------------------------------------------------------------------*
* motor control lines:
*--------------------------------------------------------------------------*/
sbit phase_0_167      = PHASE_0_167;
sbit phase_0_dir_167  = PHASE_0_DIR_167;
sbit phase_1_167      = PHASE_1_167;
sbit phase_1_dir_167  = PHASE_1_DIR_167;
sbit phase_2_167      = PHASE_2_167;
sbit phase_2_dir_167  = PHASE_2_DIR_167;
sbit phase_3_167      = PHASE_3_167;
sbit phase_3_dir_167  = PHASE_3_DIR_167;

/*-------------------------------------------------------------------------*
* current motor phase state (4 phases = 4 bit width):
*--------------------------------------------------------------------------*/
BYTE motor_state;

/*-------------------------------------------------------------------------*
* global motor control variables:
*--------------------------------------------------------------------------*/
SHORT  motor_stepindex;      /* motor step index counter (0..7)        */
USHORT motor_pos;            /* motor position counter                 */
BOOL   motor_cw;             /* motor direction: cw/ccw                */
BYTE   motor_mode;           /* operating mode: full step/half step    */
BOOL   motor_fault;          /* fault detection (FALSE=OK; TRUE=error) */
USHORT motor_range;          /* motor position range                   */

/*=========================================================================*
* global extern available variables (EXTERN):
*==========================================================================*/
extern BYTE lcd_ctrl; // shadow register for LCD data/ctrl lines because LCD 
                      // and stepping motor are sharing I2C device for I/O


/*=========================================================================*
* modul internal functions:
*==========================================================================*/

/*--------------------------------------------------------------------------*
* void motor_out()
*---------------------------------------------------------------------------*
* FT: low level output routine for motor windings
* EP: -
* RV: -
* GP: PHASE_ON, PHASE_OFF, motor_state
*---------------------------------------------------------------------------*/
void motor_out()
{
  if (TQMMOD == TQM164)
  {
    /* port output for TQM164: */
    // not yet implemented
  }
  else
  {
    /* port output for TQM165,166,167,167LC: */
    phase_0_167 = (motor_state & PHASE_0_MASK);
    phase_1_167 = (motor_state & PHASE_1_MASK);
    phase_2_167 = (motor_state & PHASE_2_MASK);
    phase_3_167 = (motor_state & PHASE_3_MASK);
  }
}

/*=========================================================================*
* extern available functions:
*==========================================================================*/

/*--------------------------------------------------------------------------*
* BYTE  motor_setdir(BYTE dir)
*---------------------------------------------------------------------------*
* FT: set stepping motor direction
* EP: dir = motor direction  MOTOR_CW: clock wise rotation
*                           MOTOR_CCW: counter clock wise rotation
* RV: TRUE = function succesfully; FALSE = function failed
* GP: 
*---------------------------------------------------------------------------*/
BYTE motor_setdir(BYTE dir)
{
  /* check parameter: */
  if ((dir != MOTOR_CW) && (dir != MOTOR_CCW)) return (FALSE);

  switch (dir)
  {
    case MOTOR_CW : motor_cw = TRUE; break;
    case MOTOR_CCW: motor_cw = FALSE; break;
  }
  
  return (TRUE);
}

/*--------------------------------------------------------------------------*
* BYTE  motor_setdir()
*---------------------------------------------------------------------------*
* FT: detect stepping motor direction
* EP -
* RV: MOTOR_CW = motor direction; MOTOR_CW = clock wise rotation
* GP: 
*---------------------------------------------------------------------------*/
BYTE motor_getdir()
{
  if (motor_cw == TRUE) 
    return (MOTOR_CW); 
  else  
    return (MOTOR_CCW); 
}

/*--------------------------------------------------------------------------*
* BYTE motor_init(BYTE mode)
*---------------------------------------------------------------------------*
* FT: init motor 
* EP: mode = operating mode  0: full step with 1 winding
*                            1: full step with 2 windings
*                            2: half step
* RV: 0 = function succesfully; 1 = unvalid mode; 2 = motor defect
* GP: 
*---------------------------------------------------------------------------*/
BYTE motor_init(BYTE mode)
{ 
  /* init motor operating mode and reference positioning sequence width: */
  switch (mode)
  {
    case MOTOR_FSTEP1: motor_mode = mode; break;
    case MOTOR_FSTEP2: motor_mode = mode; break;
    case MOTOR_HSTEP : motor_mode = mode; break;
    default          : motor_mode = MOTOR_MODE_UNDEF; return (1);
  }

  if(TQMMOD == TQM164)                        
  {
    /* init port lines for TQM164: */
    // not yet implemented   
  }
  else                        
  {
    /* init port lines for TQM165,166,167,167LC: */
    phase_0_167     = !(PHASE_ON);
    phase_0_dir_167 = PHASE_DIR_OUT;
    phase_1_167     = !(PHASE_ON);
    phase_1_dir_167 = PHASE_DIR_OUT;
    phase_2_167     = !(PHASE_ON);
    phase_2_dir_167 = PHASE_DIR_OUT;
    phase_3_167     = !(PHASE_ON);
    phase_3_dir_167 = PHASE_DIR_OUT;
  }

  /* init driver enable line: */
  motor_stepindex = 0;                           /* init motor step index */
  motor_enable();                                  /* enable motor driver */
  motor_set(PHASES_ALL_OFF);
  motor_fault = FALSE;                  /* init fault detection: no fault */
  motor_cw  = TRUE;                               /* init motor direction */

  return (0);                                     /* function succesfully */
}

/*--------------------------------------------------------------------------*
* void motor_enable()
*---------------------------------------------------------------------------*
* FT: switch on motor driver
* EP: -
* RV: -
* GP: -
*---------------------------------------------------------------------------*/
void motor_enable()
{ 
  /* enable stepping motor via shadow register and hardware register: */
  lcd_ctrl &= ~0x40;
  i2c_write(LCD_I2C_ADR, 1, lcd_ctrl);
}

/*--------------------------------------------------------------------------*
* void motor_disable()
*---------------------------------------------------------------------------*
* FT: switch off motor driver ( = standby mode)
* EP: -
* RV: -
* GP: 
*---------------------------------------------------------------------------*/
void motor_disable()
{ 
  /* disable stepping motor via shadow register and hardware register: */
  motor_set(PHASES_ALL_OFF);
  _nop_();
  lcd_ctrl |= 0x40;		              
  i2c_write(LCD_I2C_ADR, 1, lcd_ctrl);  
}

/*--------------------------------------------------------------------------*
* BOOL motor_step()
*---------------------------------------------------------------------------*
* FT: motor driver port output
* EP: -
* RV: TRUE = function succesfully; FALSE = unvalid driving mode
* GP: 
*---------------------------------------------------------------------------*/
BOOL motor_step()
{ 
  if (motor_cw == TRUE)
  {
    if (++motor_stepindex > 7) 
	{
      motor_stepindex = 0;
	}
  }
  else
  {
    if (--motor_stepindex < 0) 
	{
      motor_stepindex = 7;
	}
  }

  switch (motor_mode)
  {
    case MOTOR_FSTEP1: motor_set(fstep_1[motor_stepindex]); break;
    case MOTOR_FSTEP2: motor_set(fstep_2[motor_stepindex]); break;
    case MOTOR_HSTEP : motor_set(hstep[motor_stepindex]); break;
    default          : return (FALSE);
  }

  return (TRUE);  
}

/*--------------------------------------------------------------------------*
* void motor_set(BYTE state)
*---------------------------------------------------------------------------*
* FT: control motor phases
* EP: state = 4 bit nibble for motor phase stati
* RV: -
* GP: 
*---------------------------------------------------------------------------*/
void motor_set(BYTE state)
{
  motor_state = state;
  motor_out();
}

/*--------------------------------------------------------------------------*
* BOOL motor_getfault()
*---------------------------------------------------------------------------*
* FT: get motor error status 
* EP: -
* RV: TRUE = driver failure detected; FALSE = no driver failure detected
* GP: 
*---------------------------------------------------------------------------*/
BOOL motor_getfault()
{
  if (motor_fault == TRUE) return (TRUE);

  return (FALSE);
}