P-ROBO(ピーロボ)を作ってみよう！サブ関数群

P-ROBO(ピーロボ)を作ってみよう！

サブ関数群

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

割り込み許可関数
割り込み禁止関数
ユーザＬＥＤ制御関数
Ｗａｉｔ関数
ロータリスイッチ読み込み関数
右モータ制御関数
左モータ制御関数
赤外ＬＥＤセンサ制御関数
センサ読み込み関数
モータ動作設定関数

１．割り込み許可関数

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

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

２．割り込み禁止関数

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

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

３．ユーザＬＥＤ制御関数

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

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

４．Ｗａｉｔ関数

タイマ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 　　}
}

５．ロータリスイッチ読み込み関数

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

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

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

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

６．右モータ制御関数

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

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

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

７．左モータ制御関数

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

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

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

８．赤外ＬＥＤセンサ制御関数

赤外ＬＥＤセンサを制御する関数です。

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

９．センサ読み込み関数

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

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で無い場合はループする

}

１０．モータ動作設定関数

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

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;
　　}

}