;            プログラム名　　Pendulum.asm
;
;倒立振り子のコントロールプログラムである。
;     　　　　　　　　　　　　　　　　　　　　　　　　　　　   1999,9/4～　　金野
;
;調整のために変更しても良い箇所
;（１）pulsewidth   equ  40   ;数値を、１０前後から２００前後まで変更可
;（２）近似式　offsetdeg=deltashift*定数　　;定数を変更してみる。変更の仕方は
;　　　　　　　　　　　　　　　　　　　　　　プログラム中の next_job 以下の４行を
;　　　　　　　　　　　　　　　　　　　　　　その様にプログラムをその様に変更することになる
;
;                       振り子            |垂直線
;　　        　　　　     　　|　　　　　｜　　　　
;　　        　     　　　　　　|　　　　｜　
;             　こちらでは　　　　|　　　｜
;             　角度電圧値大　　　|　　　｜　こちらでは角度電圧値小
;　　　        　　　     　　　　　　|　｜
;                                　　　| ｜         ポテンションメータ
;       前部                           | |　　　　                       　　　　後部
;              こちらでは位置電圧値大　  |　　　　　こちらでは位置電圧値小
;        　　　車体車体車体車体車体車体車体車体車体車体車体車体車体
;　　　       　　　タイヤ　　　　　　　　　　　　　　タイヤ　　　　
;
;振り子装置のハードの概要
;（１）計測値は振り子の角度電圧値と、車体の位置電圧値の２つである。
;　　（１．１）角度電圧値は自作フォト・インタラプタでアナログ量として計測し、ＰＩＣのＡＮ０に入力。
;　　　　　＊　振り子の検出可能最大傾斜角度は光源が円形であることから中立位置から数度（約±５度）である。
;　　　　　＊　電源電圧に依存して角度電圧値の範囲特に最大電圧値が変動する。オシロスコープで確認すること。
;          ＊  振り子を垂直に維持した状態で、フォト・インタラプタの出力電圧が電源電圧の半値あたりに
;　　　　　　　あるように、振り子のスリット部を調整すること。
;　　（１．２）位置電圧値はポテンションメータでアナログ量として計測し、ＰＩＣのＡＮ１に入力。
;　　　　　＊　車体を動作開始位置においたとき、ポテンションメータの中間タップの位置が中央付近に位置する
;　　　　　　　ようにタイヤを空転させて調節する。
;（２）電源は２系統である。
;　　（２．１）制御系の電源は単３電池２本の＋３Ｖ。
;　　（２．２）モータードライブの電源は００６Ｐ電池２本直列、又は外部ＤＣ電源使用可である。
;　　　　　＊　００６Ｐ電源を用いれば、装置は完全に自立型で動作させることができる。が、
;              消費電流が多いので、極力短時間での動作とするべきである。
;　　　　　＊　外部電源を使用する場合には、００６Ｐ電池に取り付けてある電極キャップの１つは外すこと。
;（３）装置の動作モードは２種類である。
;　　モード１　振り子の傾斜角度電圧値のみ計測量として動作する。完成している。
;　　モード２　車体の位置電圧値も計測量として動作する。短時間ならば良い。
;
;    これらのどちらのモードでシステムを動作させるかはＲＢ５端子のＨ、Ｌの論理レベルで設定される。
;
;モード１での動作の場合の制御仕様
;一定時間毎に割り込みをかけ、
;（１）振り子の傾斜角度電圧値を測定し、倒れた振り子を中立位置に戻すために、その量の正負に対応
;　　　させて、モーターを順方向又は逆方向に回転させる。かつ変位量の大小に対応させてモーターの
;　　　回転量も大小させるパルスを作り、モータードライブする。
;
;モード２での動作の場合の制御仕様
;一定時間毎に割り込みをかけ、
;（１）振り子の傾斜角度電圧値と車体の位置電圧値を測定する。
;（２）このモードで使用する、振り子の中立角度電圧値 degcur は、最初は degorg と等しくしておく。
;（３）車体が原点から離れるに従って、位置変位量 deltashift が大きくなる。
;（４）deltashift と比例させて、振り子が追求する中立角度電圧値 degcur 逐次を変化させる。
;
;　　　近似式　　　offsetdeg=deltashift*定数　　　　　定数として４が良い
;
;　　　車体が前方にある時には、degcur を degorg-offsetdeg に、
;　　　車体が後方にある時には、degcur を degorg+offsetdeg に、
;　　　車体が前方、後方にあることを示すのは変数 shiftsign の第０ビット（＝１，０）である。
;
;プログラムの概要
;（２）最初振り子を手で垂直に安定に立てた状態で、スタートスイッチを押すと、その時の
;　　　振り子の傾斜角度電圧値を中立値として取り込む。そして、直ぐ手を離す。
;（３）その後、システムは一定時間毎に振り子の傾斜角度或いは車体の位置を測定し、ＰＷＭ方法で
;      モーターのオン・オフを繰り返し、倒立維持の自動制御動作に入る。
;
;入出力端子
;（１）振り子の傾斜角度電圧は、ＡＮ０端子に入力。
;（２）スタートスイッチは、ＲＢ４端子に接続している。
;（３）モーターの制御パルスは、ＲＢ２，ＲＢ３端子から出力する。
;（４）タイマー割り込みのタイミングはＲＢ０端子でモニターできる。
;
;その他の仕様
;（１）振り子が倒れすぎた時には、駆動モーターをオフとする。
;（２）振り子の傾斜の中立位置での電圧値と、現時点での電圧値の差を求めその差の大小に応じて、
;　　　モーター駆動パルス幅を変更（ＰＷＭ法）し、トルクの調整を行う。即ち、振り子の傾き
;　　　が大きいときには、大きなトルクで、傾きが小さいときには、小さいトルクでモーターを
;　　　オンする。
;（３）モード２では振り子の傾斜の中立値は車体の位置変位量に応じて、変化させる方法を採用している。
;
;
;本倒立振り子装置の通常的な動かし方
; 　 外部ＤＣ電源を使用するかしないかが最初の選択である。
;　 　 （１）外部電源を使用する場合には、００６Ｐ電池に電極キャップが取り付けられている場合には
;　　　  　　少なくとも片方のキャップを取り外してから、車体の脇にでている電源コードに極性を
;　　　　  　間違わないよう外部電源のコードを接続する。
;　 　 （２）車載電池（００６Ｐ電池）を使用する場合には、電極キャップがはずれているならば、それを
;　　　  　　取り付ければよい。
;　　動作モードは２つありそれを選択する。 
;　　モード１で動作させる場合
;　　　（１）ポテンションメータの歯車がはずれていることを確認する。
;　　　（２）ＰＩＣ回路部の電源スイッチ（車体の前部にある）をオンとする。
;　　　（３）モータードライバ部の電源スイッチ（胴体の下にある）をオンとする。
;　　　（４）Ｒｅｓｅｔスイッチを押す。
;　　　（５）手などで振り子を垂直の平衡状態に保持しながら、Ｓｔａｒｔスイッチを押す。
;　　　（６）装置は動作を開始し、振り子から手を離しても、振り子を倒させまいとして、車体は前後に動く。
;
;　　振り子が倒れすぎると、安全のためモーターは自動的にオフとなる。動作を再開させたければ、振り子
;　　を垂直状態まで持っていけば、自動的に動作を再開する。
;　　動作を終了したければ、強制的に振り子を傾けるか、Ｒｅｓｅｔスイッチを押せばよい。その後、電源
;　　スイッチをオフとする。
;
;　　モード２で動作させる場合
;　　　（１）ポテンションメータの歯車がとりついていることを確認する。
;　　　（２）動作を開始する地点で、ポテンションメータの中央端子が中間付近に位置するように
;　　　　　　車輪を空回しして調節する。
;　　　（３）後はモード１の場合の（２）以下と同じ操作となる。
;
;
;アセンブル時の注意
;（１）定数はリテラル値扱いである。厳重に留意しておくこと。
;（２）Radix は Decimal とする。暗黙数値は１０進数と見なす。
;（３）アセンブルした出力ファイルは　INHX32 形式に指定すべし

  LIST P=PIC16C711       ;使用するプロセッサを指定する
  #INCLUDE "P16C711.INC" ;上記プロセッサのためのインクルードファイルを
                         ;呼び出し結合する
;ＰＩＣの初期設定 
  __CONFIG  _WDT_OFF & _XT_OSC &_BODEN_OFF
                          ;ＰＩＣはマイクロコントローラモード
                          ;これでポートＣその他の端子がＩ／Ｏポートとなる。
                          ;ウオッチドックタイマーはオフ
                          ;発振はクリスタル方式
                          ;ブラウンアウトはオフ。（オンでは規定電圧以下となるとリセット）

;************************************************************************************************ 
;変数	
       CBLOCK h'000C'     ;このディレクチブと下のENDCの間に使用する変数名を定義できる
                          ;数値は使用するプロセッサの一般目的レジスタの開始番地とする　　
            degorg        ;振り子が垂直安定状態の時の角度電圧値が入る変数　
            degnow        ;最新のＡＤ変換して得られた角度電圧値の入る変数
            degcur        ;車体の変位に依存して、振り子の新しい中立角度電圧値が入る変数。
            deltadeg      ;差角度電圧値の入る変数。相対値である　
            offsetdeg     ;初期の振り子の中立角度電圧値 degorg にオフセットする角度変数量
            degsign       ;この変数の第０ビット目に差角度電圧値が正値の時は１，負値の時は０を入れる　

            shiftorg      ;車体の原点位置のを与える位置電圧値が入る変数
            shiftnow      ;最新のＡＤ変換して得られた位置電圧値の入る変数
            deltashift    ;差位置電圧値の入る変数。相対値である
            shiftsign     ;この変数の第０ビット目に差位置電圧値が正値の時は１，負値の時は０を入れる。
            temp0         ;一時的カウンタ変数
            temp1         ;
            temp2         ;
            temp3         ;
       ENDC

pulsewidth    equ      120; 40;サンプリング時間　１０ＭＨｚで約０．８ｍｓ　
                          ;　００１（１：４）  
;振り子が倒れ過ぎた時、動作を停止させるための角度電圧値の設定
;      ＰＩＣのＡＤ変換器は０．０Ｖ～電源電圧Ｖｄを数値（０～２５５）に変換するが、モニターした振
;      り子の角度電圧値が以下の範囲を超えたときには、振り子が倒れすぎているので安全のためモーター
;　　　をオフさせる
lowdeg        equ     15   ;これらは動作を確認しながら適宜値として良い
higdeg        equ   240   ; 

;車体が動きすぎたとき、動作を停止させるための位置電圧値の設定
;　　　車輪の回転をモニターしているポテンションメータの可動範囲が限定されており、車体が動きすぎる
;　　　とこのポテンションメータが動かなくなってしまう。そうなる前に安全のため、モーターをオフさせる。
;　　　ＰＩＣのＡＤ変換器は０．０Ｖ～電源電圧Ｖｄを数値（０～２５５）に変換するが、ポテンションメータ
;　　　で測定した位置電圧値が以下の範囲を超えたときには、車体が動き過ぎているとする。
lowshift      equ     10  ;これらは動作を確認しながら適宜値として良い。
higshift      equ   245   ; 

limshift      equ    25   ;振り子の中立角度の変更量は車体の位置変化量に比例させている。
                          ;従って、車体が動きすぎると、中立角度の変更量がオーバーしてしまう場合が
                          ;生ずる。これを回避するために、車体の位置変化量が、ここで指定した値を
                          ;過ぎたときは、角度変更量は全てこの時の値とする。
;
;************************************************************************************************
          org     0                ;スタートアドレス０番地
          goto    start

;*********** 割り込み処理 ***********************************************************************

          org     4             ;割り込みベクタのある番地
handler
          bsf     PORTB,1       ;割り込みモニター端子をＨレベルとする。
          nop
          nop
          nop
          nop 
          nop
          nop
          nop
          bcf     PORTB,1       ;割り込みモニター端子をＬレベルとする。

;モードの選択状況の判断
          btfss   PORTB,5       ;ＲＢ５端子のレベルがＨの時、モード１の処理を行い、Ｌの時には  
          goto    mode2         ;ジャンプして、モード２の処理を行う。

;****************************************************
;モード１の処理 
;このモードでは角度のみの処理を行い振り子を制御する 
mode1 
          movf    degorg,0       ;変数 degorg の値をＷレジスタに取り込み、
          movwf   degcur         ;変数 degcur に代入。
          call    degproc        ;
          goto    handend

;****************************************************
;モード２の処理  
;
mode2
        ;得られた車体の現時点での位置情報（ deltashift, shiftsign）により、振り子の中立電圧値
        ;を変更する。
        ;定時刻割り込みが発生する毎に、角度の測定、位置の測定、角度の測定、位置の測定、．．．
        ;が繰り返される。
        ;振り子の角度調整のためのモーター駆動は、振り子の角度測定後に行われる。従って、第１回
        ;目では、位置情報が無いので、その値は初期設定値に従わせる。

        ;車体の位置移動が大きくなり過ぎた場合の措置。目標中立角度を指定値で押さえる。
         movlw     limshift       ;
         subwf     deltashift,0   ;deltashift-limshift を計算し、
                                  ;deltashift>limshif  ならば、 deltashift=limshift とする。
         btfss     STATUS,C       ;
         goto      next_job       ;deltashift<limshift なので、予定通りの処理へ行く。  
         movlw     limshift       ;limshift の値を
         movwf     deltashift     ;deltashift に代入する。

next_job ;近似式  offsetdeg=deltashift*4　　　*8      *2
          bcf     STATUS,C
          rlf     deltashift,1   ;deltashift を左へ回転し、deltashiftに結果を保持
          bcf     STATUS,C
          rlf     deltashift,1   ;deltashift を左へ回転し、deltashiftに結果を保持
          bcf     STATUS,C
          rlf     deltashift,0   ;deltashift を左へ回転し、Ｗレジスタに結果を保持

          movwf   offsetdeg      ;変数 offsetdeg に代入する
       
          btfss   shiftsign,0    ;車体の位置で分岐
          goto    jump2_1        ;前方＝１，後方＝０
          movf    offsetdeg,0    ;車体は前方にある
          subwf   degorg,0       ;degorg-offsetdeg を計算し、結果はＷレジスタが保持。
          movwf   degcur         ;Ｗレジスタの内容を degcur に代入。   
          goto    jump2_4        ;

jump2_1   movf    offsetdeg,0    ;車体は後方にある
          addwf   degorg,0       ;degorg+offsetdeg を計算し、結果はＷレジスタに保持。
          movwf   degcur         ;Ｗレジスタの内容を degcur に代入。
          goto    jump2_4        ;

        ;振り子の角度の検出及びモーター駆動
jump2_4
          call    degproc
   
        ;車体の位置の検出。ここでは、車体の位置変位量 deltashift と
        ;        車体が前方にあるか後方にあるかを示す shiftsign の符号を決めるだけでよい。
        ;得られた deltashift と shiftsign は、振り子の角度処理ルーチンで反映される。
          bcf     ADCON0,CHS1    ;ＡＤ変換する端子の選択。ＡＮ１端子を選択し
          bsf     ADCON0,CHS0    ;位置電圧値を取り込む 
          call    adwait         ;
          bsf     ADCON0,GO      ;ＡＤ変換を開始させる
jump30    btfsc   ADCON0,GO      ;ＡＤ変換が完了すると、ビットＧＯ＝０　となる
          goto    jump30         ;ＡＤ変換完了まで待機させる
          movf    ADRES,0        ;得られたＡＤＲＥＳの値をＷに取り込む

        ;車体の位置が指定範囲外になったときには停止させる。
          movwf   shiftnow       ;変数 shiftnow に代入する。
          movlw   higshift       ;上限位置定数をＷに取り込む
          subwf   shiftnow,0     ;shiftnow-higshift を計算する
          btfsc   STATUS,C       ;得られた位置電圧値が指定範囲外になったときには
          goto    motorstop      ;モーター停止させる処理へ飛ぶ。
          movlw   lowshift       ;下限位置定数をＷに取り込む
          subwf   shiftnow,0     ;shiftnow-lowshift を計算する
          btfss   STATUS,C       ;得られた位置電圧値が指定範囲外になったときには
          goto    motorstop      ;モーターを停止させる処理へ飛ぶ。

        ;差位置電圧値を計算する。結果は絶対値として変数 deltashift に、
                                 ;正負は shiftsign の第０ビットに１，０として示されている。 
          movf    shiftorg,0     ;shiftorg の値をＷに取り込む
          subwf   shiftnow,0     ;shiftnow-shiftorg を計算する。
          btfsc   STATUS,C       ;結果が正値と負値で場合分けする
                                 ;負数は２の補数法でＷレジスタに収まっている
          goto    plusshift      ;正値の時の処理へ飛ぶ
          goto    minusshift     ;負値の時の処理へ飛ぶ

plusshift ;正値の時の処理。車体は基準原点より前方に位置している。
          movwf   deltashift     ;正値なので、差値の入っているＷの内容をそのまま deltashift に代入する。
          bsf     shiftsign,0    ;正値を示すように、＝１とする。
          goto    handend        ;

minusshift ;負値の時の処理。車体は基準原点より後方に位置している。
          movwf   deltashift     ;Ｗの内容を deltashift に代入する
          decf    deltashift,1   ;deltashift-1 を計算し、結果は deltashift に保持
          comf    deltashift,1   ;deltashift の補数をとり、結果は deltashift に代入する。
          bcf     shiftsign,0    ;負値を示すように、＝０とする。
          goto    handend        ;

motorstop
          bcf     PORTB,2       ;ＲＢ２＝ Ｌ、ＲＢ３＝Ｌレベルとする。
          bcf     PORTB,3       ;モーターを停止させる。

handend   bcf     INTCON,T0IF   ;T0IF=0 として、割り込み要因を解消させると、
          retfie                ;割り込み処理が終了した後、
                                ;割り込みがかかった時点に復帰する

;*********** スタート *****************************************************************************
start
     ;ポートの初期設定  
          call     port_initial
     ;モーター駆動パルス出力端子の初期設定。モーター停止
          bcf      PORTB,2       ;ＲＢ２＝Ｌ、ＲＢ３＝Ｌ
          bcf      PORTB,3       ;モーター停止
     ;ＡＤの初期設定    
          call     AD_initial
     ;変数の初期設定
          clrf     deltashift    ;車体の相対変位量＝０
          bsf      shiftsign,0   ;車体が先方（＝１）、後方（＝０）にあることを示す。初期値＝１とする。

     ;振り子の平衡位置での角度電圧値と車体の原点での位置電圧値を測定し、それらを基準値として取り込む
jorg1     btfsc    PORTB,4       ;スタートボタンが押されるまで待機。
          goto     jorg1         ;押されると、ＲＢ４＝０となる。

          bcf      ADCON0,CHS1   ;ＡＤ変換する端子の選択。振り子の傾き角度のモニター端子である
          bcf      ADCON0,CHS0   ;ＡＮ０端子を選択する。
          call     adwait        ;待機
          bsf      ADCON0,GO     ;ＡＤ変換開始。
jorg2     btfsc    ADCON0,GO     ;変換完了
          goto     jorg2         ;まで待機。
          movf     ADRES,0       ;変換値を
          movwf    degorg        ;degorg に代入する。          
          call     adwait        ;待機
          bcf      ADCON0,CHS1   ;ＡＤ変換する端子の選択。車体の位置のモニター端子である
          bsf      ADCON0,CHS0   ;ＡＮ１端子を選択する。
          bsf      ADCON0,GO     ;ＡＤ変換開始。
jorg3     btfsc    ADCON0,GO     ;変換完了
          goto     jorg3         ;まで待機。
          movf     ADRES,0       ;変換値を
          movwf    shiftorg      ;shiftorg に代入する。

     ;プログラムは、下の割り込みに移るので、
     ;定時間隔の割り込み処理に入る。

     ;割り込み条件の設定
     ;  関係レジスタ　ＩＮＴＣＯＮ，ＯＰＴＩＯＮ
     ;一定時間毎にＡ／Ｄサンプリングを実行させるために、タイマー割り込みを利用する
     ;　関係レジスタ　ＴＭＲ０  このレジスタがオーバーフローすると割り込みが発生する
          movlw   b'10100000'    ;INTCON=(GIE,ADIE,T0IE,INTE,RBIE,T0IF,INTF,RBIF)
          movwf   INTCON         ;GIE=1  　割り込みを許可する
                                 ;T0IE=1   タイマー割り込みを許可する
          bsf     STATUS,RP0     ;バンク１に移る
        ;  movlw   b'11010001'    ;OPTION_REG=(RBPU,INTEDG,T0CS,T0SE,PSA,PS2,PS1,PS0)

          movlw   b'11010000'
          movwf   OPTION_REG     ;T0CS=0　　TMR0のクロックとして内部命令クロックを用いる             
                                 ;PSA=0　　 TMR0にプリスケーラを用いる
                                 ;(PS2,PS1,PS0)　上記の比率設定ピット　　
                                 ;             =001    で　１：４　　　　　　
                                 ;             =010    で　１：８　　　　　
                                 ;             =100    で　１：３２　　　　
                                 ;             =110    で　１：１２８　　　　
                                 ;             =111    で　１：２５６　
          bcf     STATUS,RP0     ;バンク０に戻す

     ;プログラム試験のため無限ループ動作をさせておく。
     ;タイマー割り込みが駆動しているので、プログラムを実行させておく必要があるので。
jumpemp    nop
           nop
           goto jumpemp

;*************　初期設定ルーチン集  **************************************************************
;ポートの初期設定
port_initial 
  ;ＲＡポートの使用状態指定レジスタＡＤＣＯＮ１はバンク１にある。
  ;ＲＡポートのＲＡ０，ＲＡ１，ＲＡ２，ＲＡ３端子をＡＤ端子に設定。参照電圧は電源電圧値とする。
  ;　　ＲＡ０　　フォトインタラプタからの出力を受ける。振り子の角度モニタ端子。
  ;　　ＲＡ１　　可変抵抗からの電圧値を受ける。車体の移動距離モニタ端子。
  ;　　ＲＡ２　　固定可変抵抗からの電圧値を受ける。車体の移動距離範囲を設定する端子。現在未使用とする。
  ;　　ＲＡ３　　未使用
          bsf     STATUS,RP0        ;バンク１に移る
          movlw   b'00000000'       ;
          movwf   ADCON1            ;上記設定となる
          movlw   b'11111111'       ;アナログ入力チャンネルは対応するＴＲＩＳビットを
          movwf   TRISA             ;入力ビットとして選択しておかなければならないので。
          bcf     STATUS,RP0        ;バンク０に戻る

  ;ＲＢポートの使用状態指定レジスタＴＲＩＳＢはバンク０にある。
  ;　　ＲＢ０　　未使用　　　　　　　　　　　　　　　出力端子にしておく
  ;　　ＲＢ１　　タイマー割り込み状態モニター端子　　出力端子
  ;　　ＲＢ２　　モータードライブパルス出力　　　　　出力端子
  ;　　ＲＢ３　　モータードライブパルス出力　　　　　出力端子
  ;　　ＲＢ４　　スタートスイッチ　　　　　　　　　　入力端子
  ;　　ＲＢ５　　モード選択スイッチ　　　　　　　　　入力端子
  ;　　ＲＢ６　　未使用　　　　　　　　　　　　　　　入力端子にしておく
  ;　　ＲＢ７　　未使用　　　　　　　　　　　　　　　入力端子にしておく
          bcf     STATUS,RP0        ;バンク選択ビットを０とし、バンク０に移る。この文は不用？
          clrf    PORTB             ;PORTB のラッチデータをクリア
          bsf     STATUS,RP0        ;バンク選択ビットを１とし、バンク１に移る
          movlw   b'11110000'       ;Ｗに２進数の11110000000を入れる
          movwf   TRISB             ;ＢポートのステータスレジスタTRISBに上の値を代入。
          bcf     STATUS,RP0        ;バンク０に戻しておく。
          return

;ＡＤコンバータの初期設定　　関係するレジスタ ADCON0
AD_initial                          ;ADCON0=(ADCS1,ADCS0, ,CHS1,CHS0,GO/DONE, ,ADON)
          bsf     ADCON0,ADCS1      ;ＡＤ変換クロック　(ADCS1,ADCS0)    
          bcf     ADCON0,ADCS0      ;                  (    0,    0)   ２＊Fosc
                                    ;                  (    0,    1)   ８＊Fosc
                                    ;                  (    1,    0) ３２＊Fosc
                                    ;                  (    1,    1)   ＲＣ 
                                    ;変換クロックは小さい程良いが、必要最低時間があるので
                                    ;マニュアルの表を参照して決定すること
                                    ;
          bsf     ADCON0,ADON       ;ＡＤ変換器をオン。

          return

;*************　振り子の角度処理ルーチン  **************************************************************
degproc
          bcf     ADCON0,CHS1   ;ＡＤ変換する端子の選択。ＡＮ０端子を選択する。
          bcf     ADCON0,CHS0   ;
          call    adwait        ;待機
          bsf     ADCON0,GO     ;ＡＤ変換を開始させる。
jumph     btfsc   ADCON0,GO     ;ＡＤ変換が完了すると、ビットGO=0 となる。
          goto    jumph         ;ＡＤ変換完了まで待機させる。
          movf    ADRES,0       ;ADRES の値をＷに取り込み、
          movwf   degnow        ;変数 degnow に代入する。
          movf    degcur,0      ;degcur の値をＷに取り込む。
          subwf   degnow,0      ;degnow-degcur の結果がＷに保持される。
                                ;結果が負の時は、２の補数法でＷに保持。
                                ;STATUS の C=1 の時は正数、C=0 の時は負数。
          btfsc   STATUS,C      ;結果により場合分けする。
          goto    pluspro       ;正数値の処理へジャンプ
          goto    minuspro      ;負数値の処理へジャンプ

pluspro  ;正数値の処理。振り子は前方に傾いている。
          movwf   deltadeg      ;差電圧値が正数なのでそのまま deltadeg へ代入する。
          bsf     degsign,0     ;傾斜角度が正値であることを示すビット。＝１
          goto    fordriv       ;車を前進させるルーチンへ行く。
minuspro  ;負数値の処理。振り子は後方に傾いている。
          movwf   deltadeg      ;Ｗの内容を deltadeg に代入。
          decf    deltadeg,1    ;deltadeg－１の計算をし、結果は deltadeg に保持。  
          comf    deltadeg,1    ;deltadeg の補数を取り、結果は deltadeg に代入する。
          bcf     degsign,0     ;傾斜角度が負値であることを示すビット。＝０
          goto    backdriv      ;車を後進させるルーチンへ行く。

fordriv
          movlw   1             ;deltadeg の値が０の時は、振り子は完全中立なので、
          subwf   deltadeg,0    ;モータードライブのパルスを作らないことにする。
          btfss   STATUS,C      ;ここでこの処置をしておかないと、deltadeg が０の時に
          goto    degend        ;変なドライブパルスが形成されてしまうので。
                                ;deltadeg は０及び正値なので、
                                ;deltadeg-1 とすれば、delta=0 の時だけ、値が負となる。
          movlw   higdeg        ;上限角度定数をＷに取り込む
          subwf   degnow,0      ;degnow-higdeg を計算する。
          btfsc   STATUS,C      ;得られた角度電圧値が指定範囲外になったときには
          goto    fstop         ;モーターを停止させる処理に飛ぶ。
          bsf     PORTB,2       ;ＲＢ２＝Ｈ、ＲＢ３＝Ｌレベルとする。
          bcf     PORTB,3       ;モーターを順回転させる。
          call    motordrive    ;順回転のパルス長設定ルーチンへ行く。
fstop     bcf     PORTB,2       ;
          bcf     PORTB,3       ;モータをオフする。
          goto    degend
backdriv
          movlw   lowdeg        ;下限角度定数をＷに取り込む
          subwf   degnow,0      ;degnow-lowdeg を計算する。
          btfss   STATUS,C      ;得られた角度電圧値が指定範囲外になったときには
          goto    bstop         ;モーターを停止させる処理に飛ぶ。
          bcf     PORTB,2       ;ＲＢ２＝Ｌ、ＲＢ３＝Ｈレベルとする。
          bsf     PORTB,3       ;モーターを逆回転させる。
          call    motordrive    ;逆回転のパルス長設定ルーチンへ行く。
bstop     bcf     PORTB,2       ;
          bcf     PORTB,3       ;モータをオフする。
          goto    degend

degend    return

;*************　パルス長設定　*******************************************************************
;差角度電圧値は変数 deltadeg に入っている。
;　　　deltadeg=0 の場合はこのルーチンは実行されないようにしておいた。
motordrive                         ;ループで時間を稼ぎ、パルス長を長くする。
          movlw   pulsewidth       ;定数 pulsewidth の値をＷレジスタに取り込み、
          movwf   temp0            ;temp0 に代入する。
jump1     movf    deltadeg,0       ;
          movwf   temp1            ;temp1 に deltadeg の値を代入する。
jump2     decfsz  temp1,1          ;temp1 の値を－１し、結果が０の時、下の命令を飛ばす。
          goto    jump2            ;
          decfsz  temp0,1          ;temp0 の値を－１し、結果が０の時、下の命令を飛ばす。
          goto    jump1          
          return

;************************************************************************************************
;正確なＡＤ変換のため、若干時間を稼ぐ
;
adwait    movlw   10               ;２５５で結構、１０でも結構であった
          movwf   temp0            ;もう少し小さくても良いかもしれない。現在１カ所にしか入れて
loop10    decfsz  temp0,1          ;いないが、他のＡＤ変換部分にも入れた方がよいかもしれない
          goto    loop10
          return

;************************************************************************************************
;時間稼ぎ
waitloop
          clrf    temp0
          clrf    temp1
loop20    decfsz  temp0,1          ;
          goto    loop20           ;
loop30    decfsz  temp1,1          ;
          goto    loop20           ;
          return

;************************************************************************************************ 

         END




