ほぷしぃ

P-ROBO(ピーロボ)

始めに /  P-ROBOプログラミングの基本 /  レジスタの設定 /  割り込み処理 /  メイン関数 /  初期化 /  サブ関数群 /  実走行

サブ関数群

ライントレースを行うにあたって作成したサブ関数を紹介します。

  1. 割り込み許可関数
  2. 割り込み禁止関数
  3. ユーザLED制御関数
  4. Wait関数
  5. ロータリスイッチ読み込み関数
  6. 右モータ制御関数
  7. 左モータ制御関数
  8. 赤外LEDセンサ制御関数
  9. センサ読み込み関数
  10. モータ動作設定関数

1.割り込み許可関数

割り込みを許可する関数です。iINTCONのビット7をセットすると、すべての割り込みの発生を許可します。

void IntrEnable()
{
  iINTCON |= 0x80;  // ビット7をセットし、全ての割り込み発生を許可する
}


2.割り込み禁止関数

割り込みを禁止する関数です。iINTCONのビット7をクリアすると、すべての割り込みの発生を禁止します。

void IntrDisable()
{
  iINTCON &= 0x7f;  // ビット7をクリアし、全ての割り込み発生を禁止する
}

3.ユーザLED制御関数

ユーザLEDを制御する関数です。iUserLedDataのビット4をクリアすると、ユーザLEDが点灯します。iUserLedDataのビット4をセットすると、ユーザLEDが消灯します。

void UserLedCtrl(int iSwitch)
{
  if(iSwitch == LED_ON){
    iUserLedData = 0x00;    // ビット4をクリアし、ユーザLEDをオンする
  }else{
    iUserLedData = 0x10;    // ビット4をセットし、ユーザLEDをオフする
  }
}

4.Wait関数

タイマNoとタイマ値が引数として渡されてきます。

void Wait(int iTimerNo, int iTimerCnt)
{
  int iTimerTmp;

  uiTimerCnt[iTimerNo] = iTimerCnt;  // タイマカウンタをセットする

  switch(iTimerNo){
    case 0:
      iFlag &= 0xfd;  // タイマ1のフラグをクリアする
      do{
        iTimerTmp = (iFlag & 0x02);  // タイマ1のフラグをチェックする
      }while(iTimerTmp != 0x02);   // タイマ1のフラグがセットされていなければループする
      break;

    case 1:
      iFlag &= 0xfb;  // タイマ2のフラグをクリアする
      do{
        iTimerTmp = (iFlag & 0x04);  // タイマ2のフラグをチェックする
      }while(iTimerTmp != 0x04);  // タイマ2のフラグがセットされていなければループする
      break;

    case 2:
      iFlag &= 0xf7;  // タイマ3のフラグをクリアする
      do{
        iTimerTmp = (iFlag & 0x08);  // タイマ3のフラグをチェックする
      }while(iTimerTmp != 0x08);  // タイマ3のフラグがセットされていなければループする
      break;

    default:
      break;v   }
}

5.ロータリスイッチ読み込み関数

ロータリスイッチ読み込み関数です。

int ReadRotarySW()
{
  int iRotarySW;  // ロータリスイッチ

  iRotarySW = (~iPORTB >> 4) & 0x0f;  // ロータリスイッチの読み込み

  return iRotarySW;  // ロータリスイッチ値を返す
}

6.右モータ制御関数

右モータを制御する関数です。

void MoveMotorR(int iMove, int iSpeed)
{
  IntrDisable();  // 割り込み禁止
  iMotorData &= 0x0C;  // 右モータ動作設定値をクリアする
  iMotorData |= iMove;  // 右モータ動作を設定する

  iR_MotorSpeed = iSpeed;  // 右モータスピードを設定する
  IntrEnable();  // 割り込み許可
}

7.左モータ制御関数

左モータを制御する関数です。

void MoveMotorL(int iMove, int iSpeed)
{
  IntrDisable();  // 割り込み禁止
  iMotorData &= 0x03;  // 左モータ動作設定値をクリアする
  iMotorData |= (iMove << 2);  // 左モータ動作を設定する

  iL_MotorSpeed = iSpeed;  // 左モータスピードを設定する
  IntrEnable();  // 割り込み許可
}

8.赤外LEDセンサ制御関数

赤外LEDセンサを制御する関数です。

void IrLedCtrl(int iSwitch)
{
  if(iSwitch == LED_ON){
    iPORTB |= 0x08;
  }else{
    iPORTB &= 0xf7;
  }
}

9.センサ読み込み関数

センサを読み込む関数です。

void ReadSensor()
{
  int iPortTmp;  // ポート取得用
  int iLoopCnt;  // ループカウンタ

  iLoopCnt = 8;  // ループカウンタに8を設定

  iSensorData = 0;  // センサデータをクリア

  IrLedCtrl(LED_ON);
  Wait(0, 1);  // 10ms待ち

  iPORTB |= 0x01;  // クロックをHighに設定

  iPORTB &= 0xfd;  // ロードモードに設定

  /*** タイミング作成 ***/
  iPORTB &= 0xfe;  // クロックをLowに設定
  iPORTB |= 0x01;  // クロックをHighに設定

  iPORTB |= 0x02;  // シフトモードに設定

  IrLedCtrl(LED_OFF);

  do{
    iSensorData = iSensorData << 1;  // センサデータを1ビットシフトする

    iPortTmp = (iPORTB & 0x04);  // データビットを取得
    if(iPortTmp == 0x04){  // データビットをチェックする
      iSensorData |= 0x01;  // データビットが1の場合はセンサデータの0ビットをセットする
    }

    /*** タイミング作成 ***/
    iPORTB &= 0xfe;  // クロックをLowに設定
    iPORTB |= 0x01;  // クロックをHighに設定

    iLoopCnt--;  // ループカウンタをデクリメント
  }while(iLoopCnt != 0);  // ループカウンタが0で無い場合はループする

}

10.モータ動作設定関数

モータの動作を制御します。

void SelectMove()
{
  switch(iSensorData){
    case 0:  /*** 000 ***/
      iMoveR = MOTOR_FORWARD;
      iMoveL = MOTOR_FORWARD;
      iSpeedR = 25;
      iSpeedL = 25;
      break;
    case 1:  /*** 001 ***/
      iMoveR = MOTOR_FORWARD;
      iMoveL = MOTOR_FORWARD;
      iSpeedR = 20;
      iSpeedL = 15;
      break;
    case 2:  /*** 010 ***/
      iMoveR = MOTOR_FORWARD;
      iMoveL = MOTOR_FORWARD;
      iSpeedR = 15;
      iSpeedL = 15;
      break;
    case 3:  /*** 011 ***/
      iMoveR = MOTOR_FORWARD;
      iMoveL = MOTOR_FORWARD;
      iSpeedR = 25;
      iSpeedL = 10;
      break;
    case 4:  /*** 100 ***/
      iMoveR = MOTOR_FORWARD;
      iMoveL = MOTOR_FORWARD;
      iSpeedR = 15;
      iSpeedL = 20;
      break;
    case 5:  /*** 101 ***/
      iMoveR = MOTOR_FORWARD;
      iMoveL = MOTOR_FORWARD;
      iSpeedR = 20;
      iSpeedL = 20;
      break;
    case 6:  /*** 110 ***/
      iMoveR = MOTOR_FORWARD;
      iMoveL = MOTOR_FORWARD;
      iSpeedR = 10;
      iSpeedL = 25;
      break;
    case 7:  /*** 111 ***/
      switch(iSensorBack){
        case 0:  /*** 000 ***/
        case 2:  /*** 010 ***/
        case 5:  /*** 101 ***/
        case 7:  /*** 111 ***/
          iMoveR = MOTOR_STOP;
          iMoveL = MOTOR_STOP;
          iSpeedR = 0;
          iSpeedL = 0;
          break;
        case 1:  /*** 001 ***/
          iMoveR = MOTOR_FORWARD;
          iMoveL = MOTOR_FORWARD;
          iSpeedR = 20;
          iSpeedL = 0;
          break;
        case 3:  /*** 011 ***/
          iMoveR = MOTOR_FORWARD;
          iMoveL = MOTOR_BACK;
          iSpeedR = 20;
          iSpeedL = 20;
          break;
        case 4:  /*** 100 ***/
          iMoveR = MOTOR_FORWARD;
          iMoveL = MOTOR_FORWARD;
          iSpeedR = 0;
          iSpeedL = 20;
          break;
        case 6:  /*** 110 ***/
          iMoveR = MOTOR_BACK;
          iMoveL = MOTOR_FORWARD;
          iSpeedR = 20;
          iSpeedL = 20;
          break;
        default:
          break;
      }
      break;
    default:
      break;
  }

}

前のページへ ページのトップへ 次のページへ