/* Motor control 
   Last updated 4.1.01
 */

#include "H8.h"
#include "OutputPorts.h"

#ifndef   MotorControl_H_DEFINED
#define   MotorControl_H_DEFINED

/* Motor control. 
 *
 * Control of motor A and C. Port B is not used.
 *
 * Every msec a timer interrupt calls MotorControlUpdate 
 * to update the state of the motors on the output ports.
 * On each update a port is set to one of the possible 
 * states: OnNeg, OnPos, Brake or Float.
 *
 * A waveform of 16 zero's and one's is used to control a
 * sequence of 16 states to a port:  0 means Float, 
 * 1 means whatever state is given to the motor 
 * on the given output port by MotorASetState and MotorCSetState.
 * The waveform is repeated and the result is different power 
 * patterns given to an output port. The waveform is set by
 * MotorASetPower and MotorCSetPower.
 *
 */

#define NoPower   0
#define Power1    0x0001    /* 0000 0000 0000 0001 */
#define Power2    0x0101    /* 0000 0001 0000 0001 */
#define Power3    0x0421    /* 0000 0100 0010 0001 */
#define Power4    0x1111    /* 0001 0001 0001 0001 */
#define Power5    0x2249    /* 0010 0010 0100 1001 */
#define Power6    0x2929    /* 0010 1001 0010 1001 */
#define Power7    0x54a9    /* 0101 0100 1010 1001 */
#define Power8    0x5555    /* 0101 0101 0101 0101 */
#define Power9    0xab56    /* 1010 1011 0101 0110 */
#define Power10   0xd6d6    /* 1101 0110 1101 0110 */
#define Power11   0xddb6    /* 1101 1101 1011 0110 */
#define Power12   0xeeee    /* 1110 1110 1110 1110 */
#define Power13   0xfbde    /* 1111 1011 1101 1110 */
#define Power14   0xfefe    /* 1111 1110 1111 1110 */
#define Power15   0xfffe    /* 1111 1111 1111 1110 */
#define Power16   0xffff    /* 1111 1111 1111 1111 */

static word Power[17] = { NoPower, Power1, Power2, Power3, Power4,
                                   Power5, Power6, Power7, Power8,
                                   Power9, Power10,Power11,Power12,
                                   Power13,Power14,Power15,Power16
                        };

struct MotorDesc_t { byte State;      /* Float, OnPos, OnNeg, Brake */
                     word WaveState;  /* Current state of waveform  */ 
                    };

typedef struct MotorDesc_t MotorDesc;

static MotorDesc MotorA;
/* Port B used for speaker */
static MotorDesc MotorC;

static byte MotorStates;
static byte PortState;

void MotorControlUpdate( void )
{
  /* Update state of motor A and C on output port */
  Disable;
  PortState = OutputPorts;
  OutputPorts = (PortState & ~( Amask | Cmask ) ) | MotorStates;
  Enable;
  
  /* Calculate next motor state for motors A and C */
  MotorStates = (Float<<6) + Float;
  if ( MotorA.WaveState & bit0 ){
       MotorStates |= MotorA.State<<6;
       MotorA.WaveState = (MotorA.WaveState >> 1) | 0x8000; /* Rotate right */
  }
  else
    MotorA.WaveState = MotorA.WaveState >> 1; /* Rotate right */

  if ( MotorC.WaveState & bit0 ){
       MotorStates |= MotorC.State;
       MotorC.WaveState = (MotorC.WaveState >> 1) | 0x8000; /* Rotate right */
  }
  else
    MotorC.WaveState = MotorC.WaveState >> 1; /* Rotate right */

}

void MotorASetPower ( byte power )
{
  MotorA.WaveState = Power[power];
}

void MotorASetState ( byte state )
{
  MotorA.State = state;
}

void MotorCSetPower ( byte power )
{
  MotorC.WaveState = Power[power];
}

void MotorCSetState ( byte state )
{
  MotorC.State = state;
}

void MotorControlStopMotors ( void )
{
  MotorA.State    = Float; MotorA.WaveState = NoPower;
  MotorC.State    = Float; MotorC.WaveState = NoPower;  
}

/* These should only be called from timer interrupt handler */

void MotorControlInit ( void )
{ 
  MotorA.State    = Float; MotorA.WaveState = NoPower;
  MotorC.State    = Float; MotorC.WaveState = NoPower;
  MotorStates     = (Float<<6) + Float;
  Disable;
  PortA(Float); PortC(Float);
  Enable;
}	

void MotorControlClose( void )
{
  Disable;
  PortA(Float); PortC(Float);
  Enable;
}
		   
		   
#endif /* MotorControl_H_DEFINED */