/* バイポーラステッピングモーターのドライブテスト
 * 正弦波波形でマイクロステップ駆動
 *  2015/7/29 ラジオペンチ　http://radiopench.blog96.fc2.com/
 */

#define input1   4  // input1 @ L298N
#define input2   5  // 
#define input3   7  // 
#define input4   8  // 
#define enableA  9
#define enableB 10

unsigned long pos = 0x40000000;   // 回転位置記録用変数 下位8ビットでマイクロステップ位置
// 0xNNNN NNNN NNPP MMMM MMMM
// PPはステップ位置（0-3）
unsigned int micro = 16;          // マイクロステップ変化量(256に対する値を指定）
int t = 2;                        // パルスレート設定　単位：ms

byte wave[16] = {  0, 25, 50, 74, 98, 121, 142, 162,        // 正弦波データ
                   181, 198, 213, 225, 236, 244, 250, 254
                };

void setup() {
  pinMode(input1, OUTPUT);
  pinMode(input2, OUTPUT);
  pinMode(enableA, OUTPUT);
  pinMode(input3, OUTPUT);
  pinMode(input4, OUTPUT);
  pinMode(enableB, OUTPUT);

  //  TCCR1B = (TCCR1B & 0xF8) | 0x03;   // Pin9,10のPWMの周波数設定
  // 0x05=30.5Hz, 0x04=122Hz, 0x03=490Hz, 0x02=3.9kHz, 0x01=31.4kHz
}

void loop() {
  for (int j = 0; j < 1; j++) {
    for (int i = 0; i < 48 * 16; i++) {
      cwP();                      // CW方向へ1マイクロステップ
    }
    //delay(200);
  }

  delay(500);
  for (int j = 0; j < 1; j++) {
    for (int i = 0; i < 48 * 16; i++) {
      ccwP();                     // CCW方向へ1マイクロステップ
    }
    //delay(200);
  }
  delay(500);
}

void cwP() {          // CW方向へ1パルス
  pos = pos + micro;
  microStep(pos);
}

void ccwP() {         // CCW方向へ1パルス
  pos = pos - micro;
  microStep(pos);
}

void microStep(unsigned long p) {      // マイクロステップでモーター駆動
  unsigned int pwm;
  unsigned int phase;
  pwm = p & 0xFF;             // 下位8ビットはPWMの値(micro step量）
  pwm = pwm >> 4;
  phase = (p >> 8) & 0x03;    // ビットの8,9で位相

  switch (phase) {
    case 0:
      digitalWrite(input1, HIGH);    // +A
      digitalWrite(input2, LOW);
      digitalWrite(input3, HIGH);    // +B
      digitalWrite(input4, LOW);
      analogWrite(enableA, wave[15 - pwm]);
      analogWrite(enableB, wave[pwm]);
      break;

    case 1:
      digitalWrite(input1, LOW);   // -A
      digitalWrite(input2, HIGH);
      digitalWrite(input3, HIGH);  // +B
      digitalWrite(input4, LOW);
      analogWrite(enableA, wave[pwm]);
      analogWrite(enableB, wave[15 - pwm]);
      break;

    case 2:
      digitalWrite(input1, LOW);   // -A
      digitalWrite(input2, HIGH);
      digitalWrite(input3, LOW);   // -B
      digitalWrite(input4, HIGH);
      analogWrite(enableA, wave[15 - pwm]);
      analogWrite(enableB, wave[pwm]);
      break;

    case 3:
      digitalWrite(input1, HIGH);   // +A
      digitalWrite(input2, LOW);
      digitalWrite(input3, LOW);    // -B
      digitalWrite(input4, HIGH);
      analogWrite(enableA, wave[pwm]);
      analogWrite(enableB, wave[15 - pwm]);
      break;

    default:
      ;
  }
  delay(t);
}