$programmer = 22 'ARDUINO (using stk500v1 protocol) ' ' ********************************************* ' * Plotclock * ' * cc - by Johannes Heberlein 2014 * ' * Program porting O-Family 2021. 1.28 * ' ********************************************* ' ' Porting Ver 0.01 2021. 1.28 ' $regfile = "m328pdef.dat" '使用するAVRを設定。 $crystal = 16000000 'AVRクロックを設定。 ' $hwstack = 128 'ハードウェア・スタックの容量を設定。 $swstack = 128 'ソフトウェア・スタックの容量を設定。 $framesize = 128 'フレーム領域の容量を設定。 ' ' * ポート名の定義 * ' Servo_left_port Alias Portc '左サーボの制御ピンを接続するポート名。 Servo_left_bit Alias 3 '左サーボの制御ピンを接続するビット番号。 ' Servo_right_port Alias Portc '右サーボの制御ピンを接続するポート名。 Servo_right_bit Alias 4 '右サーボの制御ピンを接続するビット番号。 ' Servo_lift_port Alias Portc 'リフトサーボの制御ピンを接続するポート名。 Servo_lift_bit Alias 5 'リフトサーボの制御ピンを接続するビット番号。 '================================================================================================== ' *************************** ' * [Plotclock]の個体差設定 * ' *************************** ' 'キャリブレーション・モードの選択。 Const Calibration_mode = 0 '0:通常動作, 1:アームとリフトの組み立て時, 2:サーボとアームの調整, 3:イレーザーの待機位置調整 ' Plotclock ' cc - by Johannes Heberlein 2014 ' v 1.02 ' thingiverse.com/joo wiki.fablab-nuernberg.de ' units: mm; microseconds; radians ' origin: bottom left of drawing surface ' time library see http:'playground.arduino.cc/Code/time ' RTC library see http:'playground.arduino.cc/Code/time ' or http:'www.pjrc.com/teensy/td_libs_DS1307RTC.html '変更ログ： ' 1.01 "https:'github.com/9a/plotclock"のjooによるリリース ' 1.02 追加機能は、Dave(https:'github.com/Dave1001/)によって、以下が追加されました： ' - 左右のサーボに対して、個別にサーボ係数を校正する機能が追加されました。 ' - DS1307、DS1337およびDS3231リアルタイム・クロック・チップをサポートするためのコードを追加しました。 ' - リアルタイム・クロックを接続する方法については、http:'www.pjrc.com/teensy/td_libs_DS1307RTC.htmlを参照してください。 ' 1.03 servo2の角演算で、長さのバグ（その他の改造）を修正しました。 ' ' * キャリブレーション・モード[2] サーボとアームの調整 * ' '左右のサーボの0度(水平)位置。 'キャリブレーション・モード[1]で、左右のサーボ・アームが水平の位置に移動するように、以下の係数を調整する。 '値を小さくすると右へ回転、大きくすると左へ回転。 Const Servoleftnull = 1845 '左サーボの水平位置。初期値(1950) Const Servorightnull = 760 '右サーボの水平位置。初期値(815) '左右のサーボの90度(垂直)位置。 'キャリブレーション・モード[1]で、左右のサーボ・アームが垂直の位置に移動するように、以下の係数を調整する。 Const Servofaktorleft = 638 '左サーボの垂直位置。値を小さくすると左へ回転、大きくすると右へ回転。初期値(600) Const Servofaktorright = 650 '右サーボの垂直位置。値を小さくすると右へ回転、大きくすると左へ回転。初期値(600) ' ' * キャリブレーション・モード[3] イレーザーの待機位置調整 * ' Const Eraserwpx = 71 'イレーザーの待機位置[X]。(距離cm) 初期値(75) Const Eraserwpy = 44 'イレーザーの待機位置[Y]。(距離cm) 初期値(47) 'リフト･サーボの位置調整。描画中のペンの筆圧を見て調整する。(小さいと上へ持ち上げる) Const Zoff = 100 'リフト･サーボの位置調整。初期値(90) 'リフト･サーボを持ち上げる位置。装置の構造により微調整が必要になる場合がある。(小さいと上へ持ち上げる) Const Lift0 = 1280 + Zoff '描画時。初期値(1110) Const Lift1 = 1100 + Zoff '数字の間。初期値(995) Const Lift2 = 735 + Zoff 'イレーザーへ向かう間。初期値(735) Const Lift3 = 1100 + Zoff 'イレーズ中。 'アームを持ち上げる速度。(単位uS) Const Liftspeed = 1500 '初期値(2000) Const Wishy = 3 'ホワイトボード上でイレーザーのY座標のオフセット値。 ' ' * 装置の構造の設定 * ' 'アームの長さ。(長さcm) Const L1 = 35 'サーボ元のアームの長さ。 Const L2 = 55.1 '関節からペンまでの長さ。 Const L3 = 13.2 '左/右アームの接合部からペンまでの長さ。 Const L4 = 45 '関節から接合部までの長さ。 '原点座標(0,0)から左/右サーボの回転軸へのオフセット値。(距離cm) Const O1x = 22 '左サーボの[X]オフセット値。初期値(24/22)（大きいと左へ） Const O1y = -25 '左サーボの[Y]オフセット値。初期値(-25)（大きいと下へ） Const O2x = 47.5 '右サーボの[X]オフセット値。初期値(49/47) Const O2y = -25 '右サーボの[Y]オフセット値。初期値(-25) Const M_pi = 3.14159265358979323846 'パイ。 '描画を行うサブルーチンの定義。 Declare Sub Drawto(byval Px As Single , Byval Py As Single) Declare Sub Lift(byval Liftval As Byte) Declare Sub Return_angle(byval A As Single , Byval B As Single , Byval C As Single) Declare Sub Bogenuzs(byval Bx As Single , Byval By As Single , Byval Radius As Single , Byval Starte As Single , Byval Ende As Single , Byval Sqee As Single) Declare Sub Bogengzs(byval Bx As Single , Byval By As Single , Byval Radius As Single , Byval Starte As Single , Byval Ende As Single , Byval Sqee As Single) Declare Sub Number(byval Bx As Single , Byval By As Single , Byval Num As Integer , Byval Scale As Single) Declare Sub Set_xy(byval Tx As Single , Byval Ty As Single) '================================================================================================== ' ' * 変数の宣言 * ' Dim Liftpulse As Byte 'リフトサーボのパルス設定値。 Dim Servolift As Integer 'リフト・サーボの現在位置。 Dim Lastx As Single '現在の[X]座標。 Dim Lasty As Single '現在の[Y]座標。 ' Dim T20mscount As Byte '20mSカウンター。 Dim Mintemp As Byte '[分]更新用テンポラリ。 ' Dim Temp1 As Byte '汎用テンポラリ変数 Byte型 No.1 Dim Temp2 As Byte '汎用テンポラリ変数 Byte型 No.2 Dim Temp3 As Byte '汎用テンポラリ変数 Byte型 No.3 Dim Tempi1 As Integer '汎用テンポラリ変数 Integer型 No.1 Dim Tempi2 As Integer '汎用テンポラリ変数 Integer型 No.2 Dim Tempi3 As Integer '汎用テンポラリ変数 Integer型 No.3 Dim Temps1 As Single '汎用テンポラリ変数 Single型 No.1 Dim Temps2 As Single '汎用テンポラリ変数 Single型 No.2 Dim Temps3 As Single '汎用テンポラリ変数 Single型 No.3 Dim Tempstr As String * 20 '汎用テンポラリ変数 String型 Dim Tempstr2 As String * 20 '汎用テンポラリ変数 String型 ' '================================================================================================== ' ' * ポートの初期設定 * ' Config Servo_left_port.servo_left_bit = Output '[左]サーボの制御ピンを接続するポートを出力に設定する。 Config Servo_right_port.servo_right_bit = Output '[右]サーボの制御ピンを接続するポートを出力に設定する。 Config Servo_lift_port.servo_lift_bit = Output '[リフト]サーボの制御ピンを接続するポートを出力に設定する。 ' Config Portb.5 = Output 'ボード上のLED接続ポートを出力に設定する。 Reset Portb.5 'ボード上のLEDを消灯する。 ' ' * シリアルポートの設定 * ' Config Com1 = 9600 , Parity = None , Stopbits = 1 , Databits = 8 'ハードウェアUARTの設定。 Config Serialin = Buffered , Size = 80 'シリアルデータ受信をバッファを使用した割り込み処理にする。 ' ' * Timerの設定 * ' Config Timer1 = Timer , Prescale = 64 , Clear Timer = 1 'サーボのパルスを作成するタイマー。 Set Tccr1a.wgm11 'Timer1の設定。16,000,000Hz / 64 = 250,000Hz Set Tccr1b.wgm13 '高速PWM動作 TOP値=ICR1。 Capture1 = 5000 - 1 '250,000Hz / 50Hz(20mS) = 5000カウント On Timer1 Tintovf1 'TIMER1オーバーフロー割り込みルーチンのラベルを設定。 Enable Timer1 'TIMER1オーバーフロー割り込みを許可する。 On Compare1a Tintoc1a Nosave 'TIMER1比較一致A割り込みルーチンのラベルを設定。 Enable Compare1a 'TIMER1比較一致A割り込みを許可する。 On Compare1b Tintoc1b Nosave 'TIMER1比較一致B割り込みルーチンのラベルを設定。 Enable Compare1b 'TIMER1比較一致B割り込みを許可する。 ' Config Timer0 = Timer , Prescale = 256 'Timer0の設定。16,000,000Hz / 256 = 62,500Hz On Compare0a Tintoc0a 'TIMER0比較一致A割り込みルーチンのラベルを設定。 Stop Timer0 'Timer0を停止させる。 Set Tifr0.ocf0a 'Timer0 比較A一致フラグをリセットする。 Enable Compare0a 'TIMER0比較一致A割り込みを許可する。 ' ' * 初期設定 * ' Config Clock = User '時計（タイマー）をUserモードに設定する。 _year = 21 '日付の初期値を設定する。 _month = 1 _day = 1 ' ' * ペンをイレーザーの待機位置に移動する * ' Lift 2 'ペンを[イレーザーへ向かう間]に持ち上げる。 Drawto Eraserwpx , Eraserwpy 'ペンをイレーザーの待機位置に移動する。 ' Enable Interrupts 'すべての割り込みを許可する。 '-------------------------------------------------------------------------------------------------- ' ' * キャリブレーション・モード[1] アームとリフトの組み立て時 * ' #if Calibration_mode = 1 '較正モードの選択。(1:アームとリフトの組み立て時) Lift 0 '[リフト]サーボを[描画時]にする。 Drawto -3 , 29.2 '左アームを水平位置、右アームを垂直位置にする。 Stop #endif ' ' * キャリブレーション・モード[2] サーボとアームの調整 * ' #if Calibration_mode = 2 '較正モードの選択。(2:サーボとアームの調整) Lift 3 '[リフト]サーボを[イレーズ中]にする。 Do Drawto -3 , 29.2 '左アームを水平位置、右アームを垂直位置にする。 Wait 1 Drawto 74.1 , 28 '左アームを垂直位置、右アームを水平位置にする。 Wait 1 Loop #endif ' ' * キャリブレーション・モード[3] イレーザーの待機位置調整 * ' #if Calibration_mode = 3 '較正モードの選択。(3:イレーザーの待機位置調整) Lift 3 '[リフト]サーボを[イレーズ中]にする。 Do Lift 2 '[リフト]サーボを[イレーザーへ向かう間]にする。 Drawto Eraserwpx , Eraserwpy 'ペンをイレーザーの待機位置に移動する。 Lift 3 '[リフト]サーボを[イレーズ中]にする。 Wait 1 Lift 2 '[リフト]サーボを[イレーザーへ向かう間]にする。 Drawto 0 , 0 'ペンを原点座標(0,0)位置に移動する。 Wait 1 Loop #endif '================================================================================================== ' ' * ペンをイレーザーの待機位置に移動する * ' Lift 2 'ペンを[イレーザーへ向かう間]に持ち上げる。 Drawto Eraserwpx , Eraserwpy 'ペンをイレーザーの待機位置に移動する。 Wait 3 Mintemp = 255 '最初の描画を行う。 ' ' ******************* ' * メイン ルーチン * ' ******************* ' Main: If _min <> Mintemp Then 'If 1分経過したか? Then Gosub Timeplot '時刻をプロットする。 Mintemp = _min End If ' Temp1 = Inkey() 'シリアル受信バッファーから1バイト取り出す。 Select Case Temp1 Case &H0D : '[CR] Print Chr(temp1) 'ターミナルにエコーバックする。 Temp2 = Len(tempstr) Select Case Temp2 Case 0 : '[Enter]のみの場合。 Print "> " ; Time$ '時刻をターミナルに出力する。 Gosub Timeplot '時刻をプロットする。 Case 4 : '4文字。時刻の設定。 Tempstr2 = Left(tempstr , 2) '[時]の桁を取り出す。 _hour = Val(tempstr2) '文字列を数値に変換する。 Tempstr2 = Right(tempstr , 2) '[分]の桁を取り出す。 _min = Val(tempstr2) '文字列を数値に変換する。 _sec = 0 '秒を00にする。 Print "> " ; Time$ '時刻をターミナルに出力する。 End Select Tempstr = "" '受信文字列をクリアする。 Case Is <> 0 : '文字の場合。 Print Chr(temp1) ; 'ターミナルにエコーバックする。 Tempstr = Tempstr + Chr(temp1) '文字列にする。 End Select ' Goto Main ' ' * RTCを使用せずに[Time$]を使用するための記述 * ' Getdatetime: Return '================================================================================================== ' ' ********************** ' * 時刻をプロットする * ' ********************** ' Timeplot: Number 3 , 3 , 111 , 1 'イレーズ。 ' Temp1 = Makebcd(_hour) '10進数の[時]をBCD値に変換する。 Temp2 = Temp1 Shift Temp2 , Right , 4 Number 2 , 25.5 , Temp2 , 0.9 '[時]の上位桁。(X座標 , Y座標 , 数字 , フォントの大きさ) ' Temp1 = Temp1 And &B0000_1111 Number 16 , 26.8 , Temp1 , 0.88 '[時]の下位桁。(X座標 , Y座標 , 数字 , フォントの大きさ) ' Number 26.5 , 29 , 11 , 0.9 'コロン。 ' Temp1 = Makebcd(_min) '10進数の[分]をBCD値に変換する。 Temp2 = Temp1 Shift Temp2 , Right , 4 Number 34.5 , 29.8 , Temp2 , 0.78 '[分]の上位桁。(X座標 , Y座標 , 数字 , フォントの大きさ) ' Temp1 = Temp1 And &B0000_1111 Number 48 , 30.8 , Temp1 , 0.68 '[分]の下位桁。(X座標 , Y座標 , 数字 , フォントの大きさ) ' Lift 2 Drawto Eraserwpx , Eraserwpy 'ペンをイレーザーの待機位置に移動する。 Lift 1 Return '################################################################################################## ' ' ********************************************** ' * Timer1 オーバーフロー割り込み処理ルーチン * ' ********************************************** ' Tintovf1: Set Servo_left_port.servo_left_bit '[左]サーボの制御信号を[H]レベルにする。 Set Servo_right_port.servo_right_bit '[右]サーボの制御信号を[H]レベルにする。 ' Set Servo_lift_port.servo_lift_bit '[リフト]サーボの制御信号を[H]レベルにする。 Ocr0a = Liftpulse 'Timer0の比較器にリフト・サーボのパルス幅を設定する。 Timer0 = 0 'Timer0を[00]にクリア。 Start Timer0 'Timer0を開始する。 ' T20mscount = T20mscount + 1 '20mSカウンターを加算する。 If T20mscount > 49 Then 'If 1秒経過したか? Then T20mscount = 0 _sec = _sec + 1 '秒を更新。 If _sec > 59 Then 'If 60秒 経過したか? Then _sec = 0 _min = _min + 1 '分を更新。 If _min > 59 Then 'If 60分 経過したか? Then _min = 0 _hour = _hour + 1 '時を更新。 If _hour > 23 Then 'If 24時間 経過したか? Then _hour = 0 End If End If End If End If Return $notransform On 'R23またはR0を使用する命令に自動修正させない。 $asm ' ' **************************************** ' * Timer1 比較一致A割り込み処理ルーチン * ' **************************************** ' Tintoc1a: CBI Servo_left_port,servo_left_bit '[左]サーボの制御信号を[L]レベルにする。 RETI ' ' **************************************** ' * Timer1 比較一致B割り込み処理ルーチン * ' **************************************** ' Tintoc1b: CBI Servo_right_port,servo_right_bit '[右]サーボの制御信号を[L]レベルにする。 RETI $end Asm $notransform Off ' ' ***************************************** ' * Timer0 比較一致A 割り込み処理ルーチン * ' ***************************************** ' Tintoc0a: Reset Servo_lift_port.servo_lift_bit '[リフト]サーボの制御信号を[L]レベルにする。 Stop Timer0 'Timer0を停止する。 Return '################################################################################################## ' ' *************************************** ' * X,Y座標へ数字を描画するサブルーチン * ' *************************************** ' '左下を原点として、BXから数字を書きます。 'スケール1は、高さ20mmのフォントに等しい。 '構成はこの原則に続いています：数字の最初の開始点への移動、ペンを下げる、数字を描く、ペンを上げる。 'Number (X座標 , Y座標 , 数字 , フォントの大きさ) Sub Number(byval Bx As Single , Byval By As Single , Byval Num As Integer , Byval Scale As Single) Select Case Num Case 0 : Temps1 = 12 * Scale : Temps1 = Bx + Temps1 Temps2 = 6 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Lift 0 Temps1 = 7 * Scale : Temps1 = Bx + Temps1 Temps2 = 10 * Scale : Temps2 = By + Temps2 Temps3 = 10 * Scale Bogengzs Temps1 , Temps2 , Temps3 , -0.8 , 6.0 , 0.5 Lift 1 ' Case 1 : Temps1 = 5 * Scale : Temps1 = Bx + Temps1 Temps2 = 18 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Lift 0 Temps1 = 10 * Scale : Temps1 = Bx + Temps1 Temps2 = 21 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Temps1 = 10 * Scale : Temps1 = Bx + Temps1 Temps2 = 1 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Lift 1 ' Case 2 : Temps1 = 2 * Scale : Temps1 = Bx + Temps1 Temps2 = 13 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Lift 0 Temps1 = 8 * Scale : Temps1 = Bx + Temps1 Temps2 = 14 * Scale : Temps2 = By + Temps2 Temps3 = 6 * Scale Bogenuzs Temps1 , Temps2 , Temps3 , 4 , -0.8 , 0.9 Temps1 = 2 * Scale : Temps1 = Bx + Temps1 Temps2 = 0 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Temps1 = 13 * Scale : Temps1 = Bx + Temps1 Temps2 = 2 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Lift 1 ' Case 3 : Temps1 = 2 * Scale : Temps1 = Bx + Temps1 Temps2 = 18 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Lift 0 Temps1 = 5 * Scale : Temps1 = Bx + Temps1 Temps2 = 15 * Scale : Temps2 = By + Temps2 Temps3 = 5 * Scale Bogenuzs Temps1 , Temps2 , Temps3 , 3 , -2 , 1 Temps1 = 5 * Scale : Temps1 = Bx + Temps1 Temps2 = 5 * Scale : Temps2 = By + Temps2 Temps3 = 5 * Scale Bogenuzs Temps1 , Temps2 , Temps3 , 1.57 , -3 , 1 Lift 1 ' Case 4 : Temps1 = 9 * Scale : Temps1 = Bx + Temps1 Temps2 = 1 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Lift 0 Temps1 = 9 * Scale : Temps1 = Bx + Temps1 Temps2 = 22 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Temps1 = 1 * Scale : Temps1 = Bx + Temps1 Temps2 = 5 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Temps1 = 13 * Scale : Temps1 = Bx + Temps1 Temps2 = 6 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Lift 1 ' Case 5 : Temps1 = 3 * Scale : Temps1 = Bx + Temps1 Temps2 = 5 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Lift 0 Temps1 = 5 * Scale : Temps1 = Bx + Temps1 Temps2 = 6 * Scale : Temps2 = By + Temps2 Temps3 = 6 * Scale Bogengzs Temps1 , Temps2 , Temps3 , -2.5 , 2.5 , 1 Temps1 = 3 * Scale : Temps1 = Bx + Temps1 Temps2 = 19 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Waitms 50 Temps1 = 12 * Scale : Temps1 = Bx + Temps1 Temps2 = 20 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Lift 1 ' Case 6 : Temps1 = 2 * Scale : Temps1 = Bx + Temps1 Temps2 = 10 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Lift 0 Temps1 = 7 * Scale : Temps1 = Bx + Temps1 Temps2 = 6 * Scale : Temps2 = By + Temps2 Temps3 = 6 * Scale Bogenuzs Temps1 , Temps2 , Temps3 , 2 , -4.4 , 1 Temps1 = 9 * Scale : Temps1 = Bx + Temps1 Temps2 = 21 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Lift 1 ' Case 7 : Temps1 = 1 * Scale : Temps1 = Bx + Temps1 Temps2 = 19 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Lift 0 Temps1 = 13 * Scale : Temps1 = Bx + Temps1 Temps2 = 20 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Temps1 = 4 * Scale : Temps1 = Bx + Temps1 Temps2 = 1 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Lift 1 ' Case 8 : Temps1 = 7 * Scale : Temps1 = Bx + Temps1 Temps2 = 10 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Lift 0 Temps1 = 7 * Scale : Temps1 = Bx + Temps1 Temps2 = 15 * Scale : Temps2 = By + Temps2 Temps3 = 5 * Scale Bogenuzs Temps1 , Temps2 , Temps3 , 4.7 , -1.6 , 1 Temps1 = 7 * Scale : Temps1 = Bx + Temps1 Temps2 = 5 * Scale : Temps2 = By + Temps2 Temps3 = 5 * Scale Bogengzs Temps1 , Temps2 , Temps3 , -4.7 , 2 , 1 Lift 1 ' Case 9 : Temps1 = 9 * Scale : Temps1 = Bx + Temps1 Temps2 = 11 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Lift 0 Temps1 = 7 * Scale : Temps1 = Bx + Temps1 Temps2 = 15 * Scale : Temps2 = By + Temps2 Temps3 = 5 * Scale Bogenuzs Temps1 , Temps2 , Temps3 , 4 , -0.5 , 1 Temps1 = 6 * Scale : Temps1 = Bx + Temps1 Temps2 = 1 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Lift 1 ' Case 111 : '[イレーズ] Lift 3 'リフトサーボをイレーズ中位置にする。 ' Drawto Eraserwpx - 30 , Eraserwpy 'イレーザーフォルダーから左へ抜く。 Waitms 50 ' Drawto 3 , 52 Drawto 3 , 48 ' Drawto 63 - Wishy , 46 Drawto 63 - Wishy , 42 ' Drawto 3 , 42 Drawto 3 , 38 ' Drawto 63 - Wishy , 38 Drawto 63 - Wishy , 34 ' Drawto 3 , 34 Drawto 3 , 29 ' Drawto 63 - Wishy , 29 Drawto 65 - Wishy , 26 ' Drawto 3 , 26 Waitms 50 Drawto 60 - Wishy , 40 ' Drawto Eraserwpx + 1 , Eraserwpy 'ペンをイレーザーの待機位置に移動する。 Lift 2 ' Case 11 : '[コロン] Temps1 = 5 * Scale : Temps1 = Bx + Temps1 Temps2 = 15 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Lift 0 Temps1 = 5 * Scale : Temps1 = Bx + Temps1 Temps2 = 15 * Scale : Temps2 = By + Temps2 Temps3 = 0.1 * Scale Bogengzs Temps1 , Temps2 , Temps3 , 1 , -1 , 1 Waitms 10 Lift 1 Waitms 50 Temps1 = 5 * Scale : Temps1 = Bx + Temps1 Temps2 = 6 * Scale : Temps2 = By + Temps2 Drawto Temps1 , Temps2 Lift 0 Temps1 = 5 * Scale : Temps1 = Bx + Temps1 Temps2 = 6 * Scale : Temps2 = By + Temps2 Temps3 = 0.1 * Scale Bogengzs Temps1 , Temps2 , Temps3 , 1 , -1 , 1 Waitms 10 Lift 1 Waitms 100 ' End Select End Sub ' ' ******************************** ' * ペンをリフトするサブルーチン * ' ******************************** ' Sub Lift(byval Liftval As Byte) Select Case Liftval Case 0 : '描画時。 If Servolift >= Lift0 Then While Servolift >= Lift0 Servolift = Servolift - 1 Tempi1 = Servolift / 16 Liftpulse = Tempi1 Waitus Liftspeed Wend ' Else While Servolift <= Lift0 Servolift = Servolift + 1 Tempi1 = Servolift / 16 Liftpulse = Tempi1 Waitus Liftspeed Wend End If ' Case 1 : '数字の間。 If Servolift >= Lift1 Then While Servolift >= Lift1 Servolift = Servolift - 1 Tempi1 = Servolift / 16 Liftpulse = Tempi1 Waitus Liftspeed Wend ' Else While Servolift <= Lift1 Servolift = Servolift + 1 Tempi1 = Servolift / 16 Liftpulse = Tempi1 Waitus Liftspeed Wend End If ' Case 2 : 'イレーザーへ向かう間。 If Servolift >= Lift2 Then While Servolift >= Lift2 Servolift = Servolift - 1 Tempi1 = Servolift / 16 Liftpulse = Tempi1 Waitus Liftspeed Wend ' Else While Servolift <= Lift2 Servolift = Servolift + 1 Tempi1 = Servolift / 16 Liftpulse = Tempi1 Waitus Liftspeed Wend End If ' Case 3 : 'イレーズ中。 If Servolift >= Lift3 Then While Servolift >= Lift3 Servolift = Servolift - 1 Tempi1 = Servolift / 16 Liftpulse = Tempi1 Waitus Liftspeed Wend ' Else While Servolift <= Lift3 Servolift = Servolift + 1 Tempi1 = Servolift / 16 Liftpulse = Tempi1 Waitus Liftspeed Wend End If ' End Select End Sub ' ' ********************************************* ' * X,Y座標へ右回り円弧を描画するサブルーチン * ' ********************************************* ' 'Bogenuzs(X座標 , Y座標 , 半径 , 開始角 , 終了角 , 横の圧縮率) Sub Bogenuzs(byval Bx As Singl , Byval By As Single , Byval Radius As Single , Byval Starte As Single , Byval Ende As Single , Byval Sqee As Single) Local Inkr As Single Local Count As Single Inkr = -0.05 Count = 0 Do Temps1 = Starte + Count 'Drawto(sqee * Radius * Cos(start + Count) + Bx , Radius * Sin(start + Count) + By); Temps1 = Cos(temps1) Temps2 = Sqee * Radius Temps1 = Temps2 * Temps1 Temps1 = Temps1 + Bx ' Temps2 = Starte + Count Temps2 = Sin(temps2) Temps2 = Radius * Temps2 Temps2 = Temps2 + By ' Drawto Temps1 , Temps2 Count = Count + Inkr ' Temps1 = Starte + Count 'While((start + Count) > Ende); Loop Until Temps1 <= Ende End Sub ' ' ********************************************* ' * X,Y座標へ左回り円弧を描画するサブルーチン * ' ********************************************* ' 'Bogenuzs(X座標 , Y座標 , 半径 , 開始角 , 終了角 , 横の圧縮率) Sub Bogengzs(byval Bx As Singl , Byval By As Single , Byval Radius As Single , Byval Starte As Single , Byval Ende As Single , Byval Sqee As Single) Local Inkr As Single Local Count As Single Inkr = 0.05 Count = 0 Do Temps1 = Starte + Count 'Drawto(sqee * Radius * Cos(start + Count) + Bx , Radius * Sin(start + Count) + By); Temps1 = Cos(temps1) Temps2 = Sqee * Radius Temps1 = Temps2 * Temps1 Temps1 = Temps1 + Bx ' Temps2 = Starte + Count Temps2 = Sin(temps2) Temps2 = Radius * Temps2 Temps2 = Temps2 + By ' Drawto Temps1 , Temps2 Count = Count + Inkr ' Temps1 = Starte + Count 'While((start + Count) > Ende); Loop Until Temps1 > Ende End Sub ' ' *************************************** ' * X,Y座標へ直線を描画するサブルーチン * ' *************************************** ' 'Drawto(X座標 , Y座標) Sub Drawto(byval Px As Single , Byval Py As Single) Local Dx As Single , Dy As Single , C As Single Local I As Integer , J As Integer Dx = Px - Lastx '新しい位置のdx,dy Dy = Py - Lasty 'ミリ・メートルでの経路の長さは、4の時に1mm当たり4ステップに等しい。 Temps1 = Dx * Dx 'C = Floor(7 * Sqrt(Dx * Dx + Dy * Dy)) Temps2 = Dy * Dy Temps2 = Temps1 + Temps2 Temps1 = Sqr(temps2) C = Temps1 * 4 'Ver.01では4倍。 C = Fix(c) '小数点以下の桁を、より０に近い値にして、整数化する。 If C < 1 Then C = 1 '1より小さい場合は1にする。 I = Int(c) 'シングル型（単精度）の、整数部分を取り出す。 For J = 0 To I '点と点の直線を描画する。 Temps2 = J Temps1 = Temps2 * Dx Temps1 = Temps1 / C Temps1 = Lastx + Temps1 ' Temps2 = Temps2 * Dy Temps2 = Temps2 / C Temps2 = Lasty + Temps2 ' Set_xy Temps1 , Temps2 Next J Lastx = Px Lasty = Py End Sub ' ' ************************************** ' * cとaの間の角度のためのコサイン演算 * ' ************************************** ' Sub Return_angle(byval A As Single , Byval B As Single , Byval C As Single) Temps1 = A * A 'Return Acos((A * A + C * C - B * B) /(2 * A * C)) Temps2 = C * C Temps1 = Temps1 + Temps2 Temps2 = B * B Temps1 = Temps1 - Temps2 ' Temps2 = 2 * A Temps2 = Temps2 * C ' Temps1 = Temps1 / Temps2 Temps1 = Acos(temps1) End Sub ' ' ******************************************* ' * 座標から左/右サーボに位置の値を設定する * ' ******************************************* ' Sub Set_xy(byval Tx As Single , Byval Ty As Single) Local Dx As Single , Dy As Single , C As Single , A1 As Single , A2 As Single , Hx As Single , Hy As Single Waitus 100 ' 'ペンの間の三角形（左サーボとアームの継ぎ目の直交座標dx/dy）を計算する。 Dx = Tx - O1x '左/右サーボの原点からの位置座標を計算する。 Dy = Ty - O1y '左サーボからペン位置までの長さ(c)と、角度(a1)を計算する。 Temps1 = Dx * Dx 'C = Sqrt(Dx * Dx + Dy * Dy); Temps2 = Dy * Dy Temps2 = Temps1 + Temps2 C = Sqr(temps2) ' A1 = Atn2(dy , Dx) 'a1 = Atan2(Dy , Dx) ' Return_angle L1 , L2 , C 'a2を計算する。 A2 = Temps1 Temps1 = A2 + A1 'floor(((A2 + A1 - M_pi) * Servofaktorleft) + Servoleftnull) Temps1 = Temps1 - M_pi Temps1 = Temps1 * Servofaktorleft Temps1 = Temps1 + Servoleftnull Tempi1 = Fix(temps1) '小数点以下の桁を、より０に近い値にして、整数化する。 ' Compare1a = Tempi1 / 4 '左サーボの値。Timerは1カウント4uS。 '右サーボアームの三角形のための、連結器アーム位置を計算する。 Return_angle L2 , L1 , C A2 = Temps1 ' Temps1 = A1 - A2 'Hx = Tx + L3 * Cos((A1 - A2 + 0.621) + M_pi) '36,5° Temps1 = Temps1 + 0.621 Temps1 = Temps1 + M_pi Temps1 = Cos(temps1) Temps1 = L3 * Temps1 Hx = Tx + Temps1 ' Temps1 = A1 - A2 'Hy = Ty + L3 * Sin((A1 - A2 + 0.621) + M_pi) Temps1 = Temps1 + 0.621 Temps1 = Temps1 + M_pi Temps1 = Sin(temps1) Temps1 = L3 * Temps1 Hy = Ty + Temps1 'ペンの接合部の三角形（右サーボとアームのつなぎ目）を計算する。 Dx = Hx - O2x Dy = Hy - O2y ' Temps1 = Dx * Dx 'C = Sqrt(Dx * Dx + Dy * Dy); Temps2 = Dy * Dy Temps2 = Temps1 + Temps2 C = Sqr(temps2) ' A1 = Atn2(dy , Dx) 'A1 = Atan2(Dy , Dx) Return_angle L1 , L4 , C A2 = Temps1 Temps1 = A1 - A2 'floor(((a1 - a2) * SERVOFAKTORRIGHT) + SERVORIGHTNULL) Temps1 = Temps1 * Servofaktorright Temps1 = Temps1 + Servorightnull Tempi1 = Fix(temps1) '小数点以下の桁を、より０に近い値にして、整数化する。 ' Compare1b = Tempi1 / 4 '右サーボの値。Timerは1カウント4uS。 End Sub End