アセンブラはMPASMを使用しています。
;**************************************************************************** #DEFINE SWITCH PORTA, 4 ; スイッチ #DEFINE PORT_LED PORTA ; LED接続ポート #DEFINE PORT_SIDE_LED PORTB, 1 ; サイドLED #DEFINE PORT_MOTOR PORTB ; モーター制御出力ポート ; モーター出力 TempWorkingReg ; Wレジスタ退避用 TempStatusReg ; STARUSレジスタ退避用 StateOfPWM ; PWM状態 StateOfSensor ; センサー入力状態 StataOfDirection ; 進行方向 状態 ; センサー状態判別 CheckStateOfSensor call CourseOut ; コースアウト? call Center ; 中央? call LeftSide ; 左寄り? call RightSide ; call LittleLeftSide ; call LittleRightSide ; call Acceleration ; 加速する movlw GAIN_C btfss DIRECTION ; 進行方向は?( 0:右旋回 ) goto $+3 call TurnRight ; 逆に旋回する(回転半径を減少させる) return bsf StataOfDirection, LEFT_SIDE_FLAG ; 左寄りにセット ; (コースアウト方向の判別用) movlw GAIN_B btfsc StataOfDirection, COURSE_OUT_FLAG ; コースアウトからの復帰? movlw GAIN_A ; ゲイン変更 call TurnRight ; 右に旋回 ; やや左寄り LittleLeftSide M_CHECK POSI_LITTLE_LEFT btfss StataOfDirection, STRAIGHT_FLAG ; 直前は直進状態? goto $+4 movlw GAIN_A ; 直進からやや左に寄ったならば call TurnRight ; 右に旋回する bcf StataOfDirection, LEFT_SIDE_FLAG ; 右寄りにセット movlw GAIN_B btfsc StataOfDirection, COURSE_OUT_FLAG ; コースアウトからの復帰? movlw GAIN_A ; ゲイン変更 call TurnLeft ; 左に旋回 ; やや右寄り LittleRightSide M_CHECK POSI_LITTLE_RIGHT btfss StataOfDirection, STRAIGHT_FLAG ; 直前は直進状態? goto $+4 movlw GAIN_A ; (左寄りと逆の理論) call TurnLeft ; CourseOut movf StateOfSensor, W sublw POSI_COURSE_OUT btfsc STATUS, Z ; コースアウト? goto $+4 clrf CourseOutTime ; コースアウトでなければ clrf CourseOutTime + 1 ; コースアウト時間をクリア return btfsc CourseOutTime + 1, TIME_LIMIT_FLAG ; 復帰したか? return ; すでに復帰不可能なので何もしない bsf StataOfDirection, COURSE_OUT_FLAG ; コースアウト判定フラグ btfss STATUS, C goto _ReturnCourse incf CourseOutTime + 1, F btfss CourseOutTime + 1, TIME_LIMIT_FLAG ; 制限時間を超えたか? goto _ReturnCourse call InitializeRunState ; 復帰不可能.停止する return ; コースアウト復帰処理 _ReturnCourse call Slowdown ; 減速 btfss StataOfDirection, LEFT_SIDE_FLAG ; コースアウトした方向は? goto _ReturnCourseFromRightSide ; 左へそれた btfsc DIRECTION ; 進行方向は?( 0:右旋回 ) call CorrectStraightDirection ; 左旋回中に左へそれた.直進する movlw GAIN_C ; 右旋回中に左へそれた call TurnRight return _ReturnCourseFromRightSide ; 右へそれた btfss DIRECTION ; 進行方向は?( 0:右旋回 ) call CorrectStraightDirection ; 右旋回中に右へそれた.直進する movlw GAIN_C ; 左旋回中に右へそれた call TurnLeft return ; 右旋回 TurnRight call MultiplyGain ; ゲインを速さに比例して増加させる bcf INTCON, T0IE ; タイマ0割り込み 禁止 ; (データ更新中の処理を防ぐ) addwf Direction, F ; 右方向への角度を増加させる btfsc STATUS, C ; 桁上がり発生? incf Direction + 1, F btfsc Direction + 1, 7 ; 負数? goto _EndProcessing movf Direction + 1, W sublw DIRECTION_LIMIT - 1 btfsc STATUS, C ; 上限に達した? goto _EndProcessing movlw DIRECTION_LIMIT - 1 ; 上限値に補正 ; 左旋回 TurnLeft call MultiplyGain ; ゲインを速さに比例して増加させる bcf INTCON, T0IE ; タイマ0割り込み 禁止 subwf Direction, F ; 右方向への角度を減少させる btfss STATUS, C ; 桁下がり発生? decf Direction + 1, F btfss Direction + 1, 7 ; 正数? goto _EndProcessing movf Direction + 1, W sublw (DIRECTION_LIMIT ^ 0xFF) btfss STATUS, C ; 下限に達した? goto _EndProcessing movlw (DIRECTION_LIMIT ^ 0xFF) + 1 ; 下限値に補正 ; ゲインの増幅 MultiplyGain movwf Temp ; Wを退避 movf Speed + 1, W ; 速さを取得 sublw 0x01 movf Temp, W btfsc STATUS, C ; 速さが1以下? return ; Wをそのまま戻す clrw ; Wをクリア addwf Speed + 1, W ; 速さ分を加算 decfsz Temp, F ; ゲイン分だけ繰り返し goto $-2 ; ( 速さ x ゲイン ) ; 加速 Acceleration bcf INTCON, T0IE ; タイマ0割り込み 禁止 ; (データ更新中の処理を防ぐ) movlw SPEED_MAX subwf Speed + 1, W btfsc STATUS, C ; 上限に達している? goto _EndProcessing incfsz Speed, F ; 下位バイトを加算 goto _EndProcessing ; 桁上がり発生? incf Speed + 1, F ; 上位バイトを加算 goto _EndProcessing ; 減速 Slowdown bcf INTCON, T0IE ; タイマ0割り込み 禁止 movlw 0x01 subwf Speed, F ; 下位バイトを加算 btfsc STATUS, C ; 桁下がり発生? goto _EndProcessing decf Speed + 1, F ; 上位バイトを減算 movlw SPEED_MIN subwf Speed + 1, W btfsc STATUS, C ; 下限を超えた? goto _EndProcessing movlw SPEED_MIN ; 下限値に補正 goto _EndProcessing _EndProcessing bsf INTCON, T0IE ; タイマ0割り込み 許可 return ; --------------------------------------------------------------------------- bcf STATUS, C ; キャリーフラグ クリア BANKSEL PORTA btfsc SENSOR_SIDE ; サイドセンサーの入力は? incf StateOfSensor, F ; rlf StateOfSensor, F ; btfsc SENSOR_IN_L ; 左センサーの入力は?( Low:感知 ) incf StateOfSensor, F ; 入力ありならインクリメント rlf StateOfSensor, F ; 左へビットシフト btfsc SENSOR_IN_C ; 中央 btfsc SENSOR_IN_R ; 右 incf StateOfSensor, F rlf StateOfSensor, W ; LEDの配置に出力を合わせる call TurnOnLed ; LEDに状態表示 return ; ------------------------------------ ; LED点灯 TurnOnLed BANKSEL PORT_LED movwf PORT_LED ; LEDに出力( Low:点灯 ) btfsc StateOfSensor, SIDE_SENSOR_FLAG goto $+3 btfss SWITCH ; 入力待ち( Low:押下 ) goto _PushingSwitch ; LED処理 movf Temp, W call TurnOnLed ; LED点灯 rlf Temp, F ; 点灯状態変数をビットシフト movlw D'5' movwf TempStatusReg ; ------------------------------------------------------------------ btfss StateOfPWM, 1 ; 状態フラグは? goto _State2 btfss StateOfPWM, 2 goto _State1 bsf StateOfPWM, 1 _ResetTimer0 xorlw 0xFF ; タイマ0はアップカウンタであるから ; ビットを反転させる BANKSEL TMR0 movwf TMR0 ; タイマ再設定 bcf INTCON, T0IF ; タイマ0割り込みフラグ クリア ; ------------------------------------------------------------------ ; レジスタ復帰 ; 片モーター駆動 DriveOneSideMotor movlw TURN_RIGHT ; モーター制御出力パターンをセット btfsc DIRECTION ; 進行方向は?( Right: MSB=0 ) movlw TURN_LEFT ; 出力パターンを再セット BANKSEL PORT_MOTOR movwf PORT_MOTOR ; ポートに出力 ; ------------------------------------ ; PWM周期の算出 SetupCycle ; 両モーター停止時間 comf Speed + 1, W ; 速さを反転させて取得 (0x0F が最速) ; 片モーター駆動時間 movf Direction + 1, W ; 進行方向の大きさをデューティに設定 movwf DriveTimeOfMotor btfss DIRECTION ; 進行方向は? goto $+3 comf DriveTimeOfMotor, F ; 左旋回中ならば、デューティは反転する incf DriveTimeOfMotor, F ; 負数であるため絶対値を求める(2の補数) movf DriveTimeOfMotor, W ; 直進している? btfsc STATUS, Z return bcf STATUS, C ; 片側7ビットの指定値を 8ビットのタイマ rlf DriveTimeOfMotor, F ; に対応させるため デューティを2倍する comf DriveTimeOfMotor, W ; 補正係数 取得 andlw 0xF0 ; 1/16倍する comf Speed + 1, W ; 速さを反転させて取得 (0x0F が最速) andlw 0x0F ; 上位ビットをクリア btfsc STATUS, Z ; 最高速度? goto _SetDriveTime ; ならばデューティは補正しない movwf IntTemp2 ; 繰り返し数 設定 movf IntTemp, W ; 補正係数をWにセット addwf DriveTimeOfMotor, F ; デューティを補正 decfsz IntTemp2, F ; ( 補正係数 x 繰り返し数 )だけ加算する goto $-2 movlw 0xFF btfsc STATUS, C ; オーバーフロー発生? movwf DriveTimeOfMotor _SetDriveTime movf StopTimeOfMotor, W subwf DriveTimeOfMotor, F ; 1度目の割り込み周期分を減ずる btfss STATUS, C ; アンダーフロー発生? clrf DriveTimeOfMotor return movwf TRISB ; PortB ; ---------------------------------------- ; A/Dコンバータ #IFDEF __16F88 BANKSEL ADCON0 bcf ADCON0, ADON ; A/D Converter [off] movwf ANSEL #ENDIF ; タイマ0 BANKSEL OPTION_REG #IFNDEF _DEBUG movlw B'10000101' ; プリスケーラ( PS:101 ) #ELSE movlw B'10000000' ; デバッグ時はプリスケーラなし #ENDIF movwf OPTION_REG ; PWM周期は 約3.3msec ; call InitializeRunState ; 走行状態 初期化 ; 走行状態の初期化 InitializeRunState bcf INTCON, T0IE ; タイマ0割り込み 禁止 clrf Direction ; 進行方向 初期化(直進) clrf Direction + 1 #ENDIF END