目次 前 次

計算

 

　正３角形の頂点に、モータを配置して、モータの回転で
　できるボールの移動Vectorを計算します。

　各頂点をTop、Left、Rightと命名します。
　（実際は、Ａ、Ｂ、Ｃでも構わないのですが、
　　わかりやすくするために、こうします。）



　Topの方向を基準にすると、Left、Rightは各々-60°、
　+60°の傾きをもちます。（正３角形なので）



　垂直（Ｙ方向）と水平（Ｘ方向）の正方向を決めてTop、
  Left、Rightの回転数を-100〜+100に規格化。
　（規格化で、PWMと回転数の対応が、わかりやすくなります。）

　この条件を元に、水平と垂直に分け、Vectorの成分を求めます。




水平方向成分

　Topは、水平方向成分を持たないので、LeftとRightの
　回転数をleftPWM、rightPWMとして、合成した水平方向
　成分を求めます。

　頂点Leftの水平方向成分は   leftPWM  x cos( 60)
　頂点Righttの水平方向成分は rightPWM x cos(120)

　合成すると ($leftPWM  - $rightPWM) / 2




垂直方向成分

　Topは、垂直方向の回転成分だけを持つので、topPWMとします。

　また、LeftとRightの回転数をleftPWM、rightPWMに。

　頂点Leftの垂直方向成分は   leftPWM  x sin( 60)
　頂点Righttの垂直方向成分は rightPWM x sin(120)

　合成すると $topPWM + sqrt(3) x ($leftPWM  - $rightPWM) / 2




計算シーケンス

　Top、Left、Rightの回転数を入力後、移動Vectorを
　算出するシーケンスを考えます。

    

  1. ３頂点の成分が、-100〜+100の範囲内であることを確認します

  2. １つでも範囲外であれば、エラーメッセージ表示後終了

  3. 水平方向成分を求めます

  4. 垂直方向成分を求めます

  5. 進行方向に対する傾きを求めます


　シーケンスを決めたので、各処理に分解
　して内容を記述。




成分範囲チェックプロシージャ

　パラメータを入力し、範囲内で１を、範囲外で０を返す
　プロシージャを定義します。

　１と０の論理値なので、Yes、Noだけでもよいのですが、
　論理積を数値の積に置換して求めると、判定処理が楽に
　なるので、数値を返します。

　Tcl/Tkは、procで定義。

  proc judge {x} {
    set result 0
    if { -100 <= $x && $x <= 100 } {
      incr result
    }
    return $result
  }

　入力パラメータをＸとして、-100〜+100であるかを判定します。

　結果の初期値を０としておき、範囲内であれば結果に１を
　加え、returnを使い返値。

　３頂点の回転数の範囲をチェックし、積を変数checkに設定。
　変数checkが０ならば、エラーメッセージで終了。１ならば
　次の動作を考えるように記述。

  set check [expr [judge $topPWM] * [judge $leftPWM] * [judge $rightPWM]]
  if { $check == 0 } {
    tk_messageBox -type ok -message "Range error!"
  } else {
    ; #
  }




水平垂直成分計算

　移動VectorのＸ成分（水平）、Ｙ成分（垂直）を求めます。

　計算式は導出してありますが、Ｘ成分、Ｙ成分を求めるときに
　必要な係数は固定値です。

　最初に定数を変数に格納しておきます。

　set deltaX 0.5
　set deltaY [expr sqrt(3) / 2.0]

　２定数deltaX、deltaYを利用し、Ｘ成分、Ｙ成分を求めます。

  set xcmp [expr $deltaX * ($leftPWM - $rightPWM)]
  set ycmp [expr $topPWM + $deltaY * ($leftPWM + $rightPWM)]




角度計算

　移動VectorのＸ成分（水平）、Ｙ成分（垂直）を求めた後
　Ｘ成分が０でなければ、左右に傾いて進みます。

　左右どちらに傾いているかと、角度を求めます。

　角度は、逆正接で求められます。

　atan($ycmp/$xcmp)

　３角関数に関係する関数を使う場合、注意が必要。

　Tcl/Tkの組込み関数では、角度の単位はラジアン。
　変換しなければなりません。

　ラジアンから度への変換には、円周率πが必要になりますが
　直接πを生成する関数はないので工夫します。

　１の逆正接がπ／４なので、ラジアンから度の変換は
　次式で計算できます。

　set ang [expr 45 * atan($ycmp/$xcmp) / atan(1)]

　左右どちらに傾いているかは、変数angの正負で判定できます。
　正であれば、右に傾いています。
　負であれば、左に傾いています。

　正負という情報があるので、傾きは０〜９０度に限定します。
　負になる場合は、９０を加えて、正にします。

  if { $ang < 0 } {
    set ang [expr 90 + $ang]
    set dir "LEFT"
  } else {
    if { $ang > 0 } {
      set dir "RIGHT"
    }
  }

　傾いている方向と角度を求めるには、以下のコードにします。

  set ang 0
  set dir ""
  if { $xcmp != 0 } {
    set ang [expr 45 * atan($ycmp/$xcmp) / atan(1)]
    if { $ang < 0 } {
      set ang [expr 90 + $ang]
      set dir "LEFT"
    } else {
      if { $ang > 0 } {
        set dir "RIGHT"
      }
    }
  }




全プロシージャ

　３頂点の回転数を利用して、移動Vectorを求める
　プロシージャは、以下となります。

proc calSet {x} {
  global topPWM leftPWM rightPWM xcmp ycmp
  global deltaX deltaY ang dir
  set check [expr [judge $topPWM] * [judge $leftPWM] * [judge $rightPWM]]
  if { $check == 0 } {
    tk_messageBox -type ok -message "Range error!"
  } else {
    # get components
    set xcmp [expr $deltaX * ($leftPWM - $rightPWM)]
    set ycmp [expr $topPWM + $deltaY * ($leftPWM + $rightPWM)]
    # calculate angle
    set ang 0
    set dir ""
    if { $xcmp != 0 } {
      set ang [expr 45 * atan($ycmp/$xcmp) / atan(1)]
      if { $ang < 0 } {
        set ang [expr 90 + $ang]
        set dir "LEFT"
      } else {
        if { $ang > 0 } {
          set dir "RIGHT"
        }
      }
    }
  }
  # draw graph
}

　プロシージャ内変数は、プロシージャ内部だけで有効です。
　プロシージャ外部で定義している変数を利用する場合には、
　global指定で、明らかにしておきます。

　移動Vectorを図示するプロシージャは、別途定義。
目次 前 次

inserted by FC2 system