// TinyC implementation for MT Motor Controller

#define S1     0   // I/O Pin 0
#define S2     1   // I/O Pin 1
#define S3     2   // I/O Pin 2

//CommandType
#define TYPE   0
#define SPEED  1
#define DIR    2
#define SWITCH 3

//ControllerType
#define CTRL0  0
#define CTRL1  1

//MotorType
#define DCM    0
#define UNI    1
#define BIP    2

//PolarityType
#define POS    0
#define NEG    1

//MotorNumberType
#define MOTOR0 0
#define MOTOR1 1

//DirectionType
#define CW     0
#define ATCW   1

//SwitchType
#define ON     0
#define OFF    1


main()
{
//   UniPolar();
   BiPolar();
//   DCMotor();
}


PutByte(ch)
{
   int   i;

   pinlow(S1);
   i = 4;
   loop {
      if(i <= 0) break;
      while(pinstate(S2));
      if(ch & 0x01) pinhigh(S3); else pinlow(S3);
      pinhigh(S1);
      ch >>= 1;
      while(!pinstate(S2));
      if(ch & 0x01) pinhigh(S3); else pinlow(S3);
      pinlow(S1);
      ch >>= 1;
      i--;
   }
   while(pinstate(S2));
   pinhigh(S1);
   while(!pinstate(S2));
   pinhigh(S3);
}

MotorCmd(p1,p2,p3,p4)
{
   PutByte(p1);
   PutByte(p2);
   PutByte(p3);
   PutByte(p4);
}

UniPolar()
{
   MotorCmd(TYPE,  CTRL0, UNI, NEG);    // for common GND  uni-polar stepping motors
//   MotorCmd(TYPE,  CTRL0, UNI, POS);  // for common +VCC uni-polar stepping motors
   MotorCmd(SPEED, CTRL0, MOTOR0, 10);

   loop {
      writes("UniPolar, Dir-A", 0);
      MotorCmd(DIR,    CTRL0, MOTOR0, ATCW);
      MotorCmd(DIR,    CTRL1, MOTOR0, CW);
      MotorCmd(SWITCH, CTRL0, MOTOR0, ON);
      MotorCmd(SWITCH, CTRL1, MOTOR0, ON);
      delay(3000);
      MotorCmd(SWITCH, CTRL0, MOTOR0, OFF);
      MotorCmd(SWITCH, CTRL1, MOTOR0, OFF);
      delay(500);

      writes("UniPolar, Dir-B", 0);
      MotorCmd(DIR,    CTRL0, MOTOR0, CW);
      MotorCmd(DIR,    CTRL1, MOTOR0, ATCW);
      MotorCmd(SWITCH, CTRL0, MOTOR0, ON);
      MotorCmd(SWITCH, CTRL1, MOTOR0, ON);
      delay(3000);
      MotorCmd(SWITCH, CTRL0, MOTOR0, OFF);
      MotorCmd(SWITCH, CTRL1, MOTOR0, OFF);
      delay(500);
   }
}

BiPolar()
{
   MotorCmd(TYPE,  CTRL0, BIP, 0);
   MotorCmd(SPEED, CTRL0, MOTOR0, 10);
   loop {
      writes("BiPolar, Dir-A", 0);
      MotorCmd(DIR,    CTRL0, MOTOR0, ATCW);
      MotorCmd(DIR,    CTRL1, MOTOR0, CW);
      MotorCmd(SWITCH, CTRL0, MOTOR0, ON);
      MotorCmd(SWITCH, CTRL1, MOTOR0, ON);
      delay(3000);
      MotorCmd(SWITCH, CTRL0, MOTOR0, OFF);
      MotorCmd(SWITCH, CTRL1, MOTOR0, OFF);
      delay(500);

      writes("BiPolar, Dir-B", 0);
      MotorCmd(DIR,    CTRL0, MOTOR0, CW);
      MotorCmd(DIR,    CTRL1, MOTOR0, ATCW);
      MotorCmd(SWITCH, CTRL0, MOTOR0, ON);
      MotorCmd(SWITCH, CTRL1, MOTOR0, ON);
      delay(3000);
      MotorCmd(SWITCH, CTRL0, MOTOR0, OFF);
      MotorCmd(SWITCH, CTRL1, MOTOR0, OFF);
      delay(500);
   }
}

DCMotor()
{
   MotorCmd(TYPE,  CTRL0, DCM, 0);
   MotorCmd(TYPE,  CTRL1, DCM, 0);
   MotorCmd(SPEED, CTRL0, MOTOR0, 10);
   MotorCmd(SPEED, CTRL1, MOTOR0, 10);

   loop {
      writes("CTRL-0, Dir-A", 0);
      MotorCmd(DIR,    CTRL0, MOTOR0, ATCW);
      MotorCmd(DIR,    CTRL0, MOTOR1, CW); 
      MotorCmd(SWITCH, CTRL0, MOTOR0, ON);
      MotorCmd(SWITCH, CTRL0, MOTOR1, ON);
      delay(2000);
      MotorCmd(SWITCH, CTRL0, MOTOR0, OFF);
      MotorCmd(SWITCH, CTRL0, MOTOR1, OFF);
      delay(500);

      writes("CTRL-0, Dir-B", 0);
      MotorCmd(DIR,    CTRL0, MOTOR0, CW);
      MotorCmd(DIR,    CTRL0, MOTOR1, ATCW);
      MotorCmd(SWITCH, CTRL0, MOTOR0, ON);
      MotorCmd(SWITCH, CTRL0, MOTOR1, ON);
      delay(2000);
      MotorCmd(SWITCH, CTRL0, MOTOR0, OFF);
      MotorCmd(SWITCH, CTRL0, MOTOR1, OFF);
      delay(500);

      writes("CTRL-1, Dir-A", 0);
      MotorCmd(DIR,    CTRL1, MOTOR0, ATCW);
      MotorCmd(DIR,    CTRL1, MOTOR1, CW);
      MotorCmd(SWITCH, CTRL1, MOTOR0, ON);
      MotorCmd(SWITCH, CTRL1, MOTOR1, ON);
      delay(2000);
      MotorCmd(SWITCH, CTRL1, MOTOR0, OFF);
      MotorCmd(SWITCH, CTRL1, MOTOR1, OFF);
      delay(500);

      writes("CTRL-1, Dir-B", 0);
      MotorCmd(DIR,    CTRL1, MOTOR0, CW);
      MotorCmd(DIR,    CTRL1, MOTOR1, ATCW);
      MotorCmd(SWITCH, CTRL1, MOTOR0, ON);
      MotorCmd(SWITCH, CTRL1, MOTOR1, ON);
      delay(2000);
      MotorCmd(SWITCH, CTRL1, MOTOR0, OFF);
      MotorCmd(SWITCH, CTRL1, MOTOR1, OFF);
      delay(500);
   }
}