●delay()を使わない方法

 これまでサーボの動きを制御するのにArduinoのdelay()関数を使ってきました。delay()関数はスケッチ内容が比較的明確で単純な確認のような目的には有用ですが、ひとつ欠点があります。delay()実行中は他の処理が止まってしまうので、少し込み入ったプログラムの場合には不具合が生じることがあります。
ここでは、delay()を使わないでサーボとbeep音をコントロールする方法の例を示しておきます。具体的には、Timer0割り込みを常時まわして時間をカウントします。
・ms10-1

#include <avr/interrupt.h>
#include <avr/io.h>
#include <Wire.h>

// action
#define STOP  0
#define FWRD  1
#define BWRD  2
#define RTRN  3
#define LTRN  4
#define RGHT  5
#define LEFT  6
uint8_t actionMode = STOP;
int16_t tempAngles[12] = {90,90,90,90,90,90,90,90,90,90,90,90};
int16_t offset[12] = {3,5,-5,15,0,0,0,0,0,5,0,5};//自機に合わせて変更

int16_t stopAngles[12] = {90,90,90,90,90,90,90,90,90,90,90,90};
int16_t leftAngles[6][13] = {
// LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time 
  { 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 8},
  { 90, 90, 90,100, 90, 90, 90, 90,100, 90, 90, 90, 8},
  {100, 90, 90,100, 90, 90, 90, 90,100, 90, 90, 90, 8},
  { 90, 90, 90, 80, 90, 90, 90, 90, 80, 90, 90, 80, 8},
  { 90, 90, 90, 80, 90, 90, 90, 90, 80, 90, 90, 90, 8},
  { 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 8}
};
int16_t rghtAngles[6][13] = {
// LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time 
  { 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 8},
  { 90, 90, 90, 80, 90, 90, 90, 90, 80, 90, 90, 90, 8},
  { 90, 90, 90, 80, 90, 90, 90, 90, 80, 90, 90, 80, 8},
  {100, 90, 90,100, 90, 90, 90, 90,100, 90, 90, 90, 8},
  { 90, 90, 90,100, 90, 90, 90, 90,100, 90, 90, 90, 8},
  { 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 8}
};
int16_t rtrnAngles[8][13] = {
// 常に右重心にしました。
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8},
  {90, 90, 90,100, 80, 90, 90, 80, 90,100,100, 90, 8}, //右重心
  {90, 90, 90,100, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90,100, 80, 90, 90, 80, 90, 80, 80, 90, 4}, //右足前
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 80, 80,100,100, 90, 90,100,100, 90, 90, 90, 8}, //右重心
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90,100,100, 90,100, 90, 90,100,100, 90, 90, 90, 4}  //左足前
};
int16_t ltrnAngles[8][13] = {
// 常に左重心にしました。
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8},
  {90, 90, 90, 80, 80, 90, 90, 80, 80,100,100, 90, 8}, //左重心
  {90, 90, 90, 80, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90, 80, 80, 90, 90, 80, 90, 80, 80, 90, 4}, //右足前
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 80, 80, 90,100, 90, 90,100,100, 90, 90, 90, 8}, //左重心
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90,100,100, 90,100, 90, 90,100,100, 90, 90, 90, 4}  //左足前
};
int16_t bwrdAngles[8][13] = { //後退
// 順番を逆にしました。
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90,100, 90, 90,100,100, 90, 90, 90, 4}, //左足前
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90, 80, 80,100,100, 90, 90,100,100, 90, 90, 90, 8}, //右重心
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 90, 90, 80, 80, 90, 90, 80, 90, 80, 80, 90, 4}, //右足前
  {90, 90, 90, 80, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90, 80, 80, 90, 90, 80, 80,100,100, 90, 8}, //左重心
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8}
};
int16_t fwrdAngles[8][13] = {
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8},
  {90, 90, 90, 80, 80, 90, 90, 80, 80,100,100, 90, 8}, //左重心
  {90, 90, 90, 80, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90, 80, 80, 90, 90, 80, 90, 80, 80, 90, 4}, //右足前
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 80, 80,100,100, 90, 90,100,100, 90, 90, 90, 8}, //右重心
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90,100,100, 90,100, 90, 90,100,100, 90, 90, 90, 4}  //左足前
};
int16_t motionAngles[8][13];
uint8_t maxRows;
uint8_t divCounter;
uint8_t keyFrame;
uint8_t nextKeyFrame;
uint8_t svFlag = 0; //30msecの割り込みを知らせる

const uint8_t interruptPin = 2;

// audio variables
const uint8_t beepPin = 8;
uint16_t bpCounter = 0;    //beep音用に時間をカウント
boolean isBeep = false;    //beep音を鳴らすかどうか

// remote
boolean  rmRecieved = 0;  //信号受信完了した
uint8_t  i;               //受信データの桁
uint8_t  rmState = 0;     //信号受信状況
uint8_t  dataCode;        //データコード(8bit)
uint8_t  invDataCode;     //反転データコード(8bit)
uint16_t customCode;      //カスタムコード(16bit)
uint32_t rmCode;          //コード全体(32bit)
volatile uint32_t prevMicros = 0; //時間計測用

ISR(TIMER0_COMPA_vect)  //Timer0による割り込み
{
  svCounter++;
  if(svCounter%50 == 0) svFlag = 1;  //1msec*50=50msec
  bpCounter++;
  // カウンターTCNT0が4usごとに加算されOCR0Aの値になると割り込みがかかるので
  // 1000HzとなるようにOCR0Aに249((249+1) x 4us = 1000us(=1000Hz))を足す
  OCR0A = TCNT0 + 249;  //datasheet p.117
}

void rmUpdate() //信号が変化した
{
  uint32_t width; //パルスの幅を計測
  if(rmState != 0){
    width = micros() - prevMicros;  //時間間隔を計算
    if(width > 10000)rmState = 0; //長すぎ
    prevMicros = micros();  //次の計算用
  }
  switch(rmState){
    case 0: //信号未達
      prevMicros = micros();  //現在時刻(microseconds)を記憶
      rmState = 1;  //最初のOFF->ON信号を検出した
      i = 0;
      break;
    case 1: //最初のON状態
      if((width > 9500) || (width < 8500)){ //リーダーコード(9ms)ではない
        rmState = 0;
      }else{
        rmState = 2;  //ON->OFFで9ms検出
      }
      break;
    case 2: //9ms検出した
      if((width > 5000) || (width < 4000)){ //リーダーコード(4.5ms)ではない
        rmState = 0;
      }else{
        rmState = 3;  //OFF->ONで4.5ms検出
      }
      break;
    case 3: //4.5ms検出した
      if((width > 700) || (width < 400)){
        rmState = 0;
      }else{
        rmState = 4;  //ON->OFFで0.56ms検出した
      }
      break;
    case 4: //0.56ms検出した
      if((width > 1800) || (width < 400)){  //OFF期間(2.25-0.56)msより長い or (1.125-0.56)msより短い
        rmState = 0;
      }else{
        if(width > 1000){ //OFF期間長い -> 1
          bitSet(rmCode, (i));
        }else{             //OFF期間短い -> 0
          bitClear(rmCode, (i));
        }
        i++;  //次のbit
        if(i > 31){ //完了
          rmRecieved = 1;
          return;
        }
        rmState = 3;  //次のON->OFFを待つ
      }
      break;
  }
}

void setup() {
  // put your setup code here, to run once:
  attachInterrupt(digitalPinToInterrupt(interruptPin), rmUpdate, CHANGE);
  // LED
  pinMode(3, OUTPUT);
  // servo
  Wire.begin();
  Wire.setClock(100000);

  init_pca9685();
  set_angle();
  
  pinMode(oePin, OUTPUT);
  digitalWrite(oePin,LOW);

  // 1000Hz割り込みのためのTimer0設定
  cli();  //割り込み抑止
  // 波形出力しない WGM01->0 WGM00->0 datasheet p.115
  TCCR0A &= ~(_BV(WGM01) | _BV(WGM00));  //Table 15-8
  // コンペアマッチ割り込み有効
  TIMSK0 |= _BV(OCIE0A);  //datasheet p.118
  // 割り込みフラグクリア
  TIFR0 |= _BV(OCF0A);  //datasheet p.118
  sei();  //割り込み抑止解除
}

void loop() {
  // put your main code here, to run repeatedly:
  if(rmRecieved){ //リモコン受信した
    detachInterrupt(digitalPinToInterrupt(interruptPin));
    rmRecieved = 0;   //初期化
    rmState = 0;      //初期化
    //図とは左右が逆であることに注意
    customCode = rmCode;    //下16bitがcustomCode
    dataCode = rmCode >> 16;  //下16bitを捨てたあとの下8bitがdataCode
    invDataCode = rmCode >> 24; //下24bitを捨てたあとの下8bitがinvDataCode
    if((dataCode + invDataCode) == 0xff){   //反転確認
      switch(dataCode){  //<-switch文を追加
        case 24:  //"2"ボタン
          actionMode = FWRD;
          memcpy(motionAngles, fwrdAngles, sizeof(fwrdAngles));
          maxRows = sizeof(fwrdAngles) / sizeof(*fwrdAngles) - 1;
          break;
        case 28:  //"5"ボタン
          actionMode = STOP;
          for(int i=0; i<12; i++) {
            tempAngles[i] = stopAngles[i];
          }
          set_angle();
          break;
        case 82:  //"8"ボタン
          actionMode = BWRD;
          memcpy(motionAngles, bwrdAngles, sizeof(bwrdAngles));
          maxRows = sizeof(bwrdAngles) / sizeof(*bwrdAngles) - 1;
          break;
        case 12:  //"1"ボタン
          actionMode = LTRN;
          memcpy(motionAngles, ltrnAngles, sizeof(ltrnAngles));
          maxRows = sizeof(ltrnAngles) / sizeof(*ltrnAngles) - 1;
          break;
        case 94:  //"3"ボタン
          actionMode = RTRN;
          memcpy(motionAngles, rtrnAngles, sizeof(rtrnAngles));
          maxRows = sizeof(rtrnAngles) / sizeof(*rtrnAngles) - 1;
          break;
        case 8:  //"4"ボタン
          actionMode = LEFT;
          memcpy(motionAngles, leftAngles, sizeof(leftAngles));
          maxRows = sizeof(leftAngles) / sizeof(*leftAngles) - 1;
          break;
        case 90:  //"6"ボタン
          actionMode = RGHT;
          memcpy(motionAngles, rghtAngles, sizeof(rghtAngles));
          maxRows = sizeof(rghtAngles) / sizeof(*rghtAngles) - 1;
          break;
        case 66:  //"7"ボタン
          digitalWrite(3, HIGH);
          break;
        case 74:  //"9"ボタン
          digitalWrite(3, LOW);
          break;
        case 22:  //"0"ボタン
          isBeep = true;
          bpCounter = 0;
          break;
        default:
          break;
      }
    }
    attachInterrupt(digitalPinToInterrupt(interruptPin), rmUpdate, CHANGE);
  }
  if(actionMode != STOP && svFlag){
    svFlag = 0;
    divCounter++;
    if(divCounter >= motionAngles[nextKeyFrame][12]) {
      divCounter = 0;
      keyFrame = nextKeyFrame;
      nextKeyFrame++;
      if(nextKeyFrame > maxRows) nextKeyFrame = 0;
    }
    for(int i=0; i<12; i++) {
      tempAngles[i] = motionAngles[keyFrame][i] +
            int8_t((motionAngles[nextKeyFrame][i] - motionAngles[keyFrame][i])
             * divCounter / motionAngles[nextKeyFrame][12]);
    }
    set_angle();
    //delay(30);
  }
  if(isBeep){
    if(bpCounter == 50)tone(beepPin,162);
    else if(bpCounter == 100)tone(beepPin,194);
    else if(bpCounter == 150)tone(beepPin,230);
    else if(bpCounter == 200)tone(beepPin,245);
    else if(bpCounter == 250)tone(beepPin,260);
    else if(bpCounter == 300)tone(beepPin,275);
    else if(bpCounter == 350)tone(beepPin,290);
    else if(bpCounter == 400)tone(beepPin,305);
    else if(bpCounter == 450)tone(beepPin,330);
    else if(bpCounter == 500){
      noTone(beepPin);
      isBeep = false;
    }
  }
}

●練習問題

歩行に合わせて音を出して(足音)ください。

if(divCounter >= motionAngles[nextKeyFrame][12]) {
      divCounter = 0;
      keyFrame = nextKeyFrame;
      nextKeyFrame++;
      if(nextKeyFrame > maxRows) nextKeyFrame = 0;
      if(nextKeyFrame == 0)tone(beepPin, 200);
      if(nextKeyFrame == 2)noTone(beepPin);
    }

チュートリアルベース
<= PAPERBOTS マイクロマシーン チュートリアル(9)