少し前に書いたロボカーの続きです。ジャイロと近接センサーを加えて木箱に組みました。ステッパー、70ミリタイヤ、前輪に相当するタミヤのボールキャスターなどは、試作品からそのままこちらに組みなおしました。
回路は次のようにしました。毎度きたない手書きですみません^^;
使用しているＩＣのブレークアウトモジュールは、センサーを含め次の4種類です。
ToFセンサーはVL6180X、ジャイロコンパスはHMC5883L、近接センサーは古典的なＩＲ反射センサーです。
ジャイロコンパスの読取は簡単ですからライブラリーを使わずＩ2Ｃプロトコルを直接扱っています。下の方に掲載するプログラムをご参照下さい。
ＩＲ近接センサーはローテクで、電流を30ｍＡも消費する（2つつけて60ｍＡ）ので、測定時にのみ電源をオンとする回路にしました。
Arduinoプログラム（後ろの方に掲載します）は全体でマイコン（ATmega328P)の容量を、プログラム域・グローバル記憶域とも40％未満の使用状態で余裕十分です。
この変更でおしゃべりロボカーは前よりだいぶお利口になりました。次の動画は迷路の出口を自分でみつける様子です。
躯体には百均にある木箱をそのまま使いました。
側板の真ん中にあるスリットをステッパーの取付穴としてそのまま利用しました。
蓋やグリルなどとして、加工しやすいシナ合板を適当にカットしながら木ネジとボンドで簡単にとりつけました。
前方の蓋をあければマイコンが簡単に取り出せます。当面は無圧ソケットで下駄をはいています。後ろの箱はデジタルアンプ入りスピーカーボックスで、前回の試作品をそのままこちらに取り付けたもの。
ステッパーは後ろの方にありますが、メンテナンスができるように一応は蓋をネジ止めとしました。相手は厚さ4ミリの側板なので壊さないように注意して下穴をあけないといけませんが。
全体の大きさは長さ19センチ、幅はＩＲ近接センサーの端から端まｓでで14センチほどです。
基板は次のようにデザインしました。生基板をCNCで切削しますので、15分ほどでできます。ブレッドボードの状態でテスト済み、かつ単純なので部品取付後一発で問題なく稼働しました。
べたグランドですが、それがループにならないようにするためカットしています。ドライブＩＣでサージ吸収が行われていますが、ステッピングモーターとマイコンが電源を共有しますので雑音防止のためです。
今回はスピーカーボックス側にアンプを仕込んだものを使うために、アンプは基板にとりつけていません。
次回作る際はこれに音声用デジタルアンプも直接取り付けます。今回は基板組みあがりの写真を撮るのを忘れてくみこんでしまいましたが、次回作った際に部品取り付け状態もご紹介したいと思います。
省エネのために次のことをしていますので簡単にご紹介します。
1）ステッパー停止時の電流断
ステッパー停止時には電流を止める必要があります。止めないと無駄なだけでなく、コイルの抵抗成分は10数Ωしかないため直流状態ではショートに近くなり、ステッパーとドライブICは過熱しますので注意が必要です。一瞬だけブレーキを掛けたいときは別ですが。
ここでは5線式ユニポーラ―型で2相励磁をしていますが、停止時には電流を素直に止めています。
なお、余談ですが4線式バイポーラー型をイージードライバーやA4988モジュールなど経由で楽に接続する場合も同じことで、停止する際はステップ信号を止めるだけでなく、Sleep信号を送るなどの停止処理が必要です。マイコンの本などでステッパー停止時に流しっぱなしの例が多く見られますが注意が必要と思います。
また、トルクの許す限りフェイズ間隔を短くする（ドライブの周波数を上げる）と、インピーダンスが高くなり消費電流が減ります。
2）ＩＲ近接センサーの省エネなど
赤外LEDに常時30ｍAも流す構造となっています。2つ使えば60ｍAも消費するため工夫が必要です。ここでは、測定時にだけ電流を流す回路（ダーリントン型トランジスタならなんでもよい）とプログラムにしています。
ついでですが、検知距離はVＲで調節でき、テストすると8cm程度まで検知可能です。ただ数十円の安価な装置なので、特別な温度補正などがされていないように見えますので、感度をぎりぎりまで高めるのは誤作動の元かと思います。また太陽光が当たる場所の近くなどでは誤作動することがあるようです。
実際、上の動画では最後のほうで出口へ素直に向かうべきところ、窓に近い位置でIRを誤検出していったん逆へ曲がったようです。同じ論理でのシミュレーションと比較するとそこが異なりますから。
次の写真はＩＲ近接センサーのＶＲ調整の様子です。上から板などを近づけることで、検知距離以下でH信号がＬ信号に変わります。この装置で簡単に調整してからとりつけます。
ところで、迷路に出口をつけないとこのロボットはいつまでも出口を探し続けます。昼飯時に動かし始めたことをすっかり忘れて、夕方に戻って気づいたらまだ動き続けていました。忍耐強いものです。消費電流は平均80ｍＡ程度なのでアルカリ電池は長時間もちます。
そのロングランで、タミヤのボールキャスターががたつくようになり、動画のとおりカタカタとうるさい状態です。そのうちにボールキャスターは交換したいと思いますが、この際、物流用のボールキャスターも試そうと考えています。
Arduinoのプログラムは次のとおりです。
/* Talking car-robot with 5-wire steppers x 2
* V 1.0 (c)Akira Tominaga. May 26th, 2019
* using Talkie library： Copyright 2011 Peter Knight
* released under GPLv2 license.
* Function:
* Runs around, sensing and avoiding walls.
* Adjust directions from time to time.
* Talks about actions of itselves.
*
* Sensors attached;
* 1) Time-of-Flight to measure distance
* 2) Gyro-compass to recognize direction
* 3) IR-reflection obstacle-sensors
*/
#include // Wire.h I2C interface for Giro and ToF
#include // VL6180X.h ToF distance sensor
#include // talkie.h Talk via PWM
// Pin # assignment
// D3 PWM for Talkie
// D4 to D7 for stepper-1 (5 wire unipolar)
// D8 to D11 for stepper-2 (5 wire unipolar)
#define Led 13 // D13 for LED
// D14 to D16 (A0 to A2) for Obstacle sensors
#define obsPwr 14 // Power (on=H) for obstacle sensors
#define obsR 15 // obstacle sensor for right side
#define obsL 16 // obstacle sensor for left side
VL6180X tOF; // "tOF" as time-of-flight sensor name
Talkie sPK; // "sPK" as talkie name
// talkie constants
const uint8_t spZERO[] PROGMEM = {0x69, 0xFB, 0x59, 0xDD, 0x51, 0xD5, 0xD7, 0xB5, 0x6F, 0x0A, 0x78, 0xC0, 0x52, 0x01, 0x0F, 0x50, 0xAC, 0xF6, 0xA8, 0x16, 0x15, 0xF2, 0x7B, 0xEA, 0x19, 0x47, 0xD0, 0x64, 0xEB, 0xAD, 0x76, 0xB5, 0xEB, 0xD1, 0x96, 0x24, 0x6E, 0x62, 0x6D, 0x5B, 0x1F, 0x0A, 0xA7, 0xB9, 0xC5, 0xAB, 0xFD, 0x1A, 0x62, 0xF0, 0xF0, 0xE2, 0x6C, 0x73, 0x1C, 0x73, 0x52, 0x1D, 0x19, 0x94, 0x6F, 0xCE, 0x7D, 0xED, 0x6B, 0xD9, 0x82, 0xDC, 0x48, 0xC7, 0x2E, 0x71, 0x8B, 0xBB, 0xDF, 0xFF, 0x1F};
const uint8_t spONE[] PROGMEM = {0x66, 0x4E, 0xA8, 0x7A, 0x8D, 0xED, 0xC4, 0xB5, 0xCD, 0x89, 0xD4, 0xBC, 0xA2, 0xDB, 0xD1, 0x27, 0xBE, 0x33, 0x4C, 0xD9, 0x4F, 0x9B, 0x4D, 0x57, 0x8A, 0x76, 0xBE, 0xF5, 0xA9, 0xAA, 0x2E, 0x4F, 0xD5, 0xCD, 0xB7, 0xD9, 0x43, 0x5B, 0x87, 0x13, 0x4C, 0x0D, 0xA7, 0x75, 0xAB, 0x7B, 0x3E, 0xE3, 0x19, 0x6F, 0x7F, 0xA7, 0xA7, 0xF9, 0xD0, 0x30, 0x5B, 0x1D, 0x9E, 0x9A, 0x34, 0x44, 0xBC, 0xB6, 0x7D, 0xFE, 0x1F};
const uint8_t spTWO[] PROGMEM = {0x06, 0xB8, 0x59, 0x34, 0x00, 0x27, 0xD6, 0x38, 0x60, 0x58, 0xD3, 0x91, 0x55, 0x2D, 0xAA, 0x65, 0x9D, 0x4F, 0xD1, 0xB8, 0x39, 0x17, 0x67, 0xBF, 0xC5, 0xAE, 0x5A, 0x1D, 0xB5, 0x7A, 0x06, 0xF6, 0xA9, 0x7D, 0x9D, 0xD2, 0x6C, 0x55, 0xA5, 0x26, 0x75, 0xC9, 0x9B, 0xDF, 0xFC, 0x6E, 0x0E, 0x63, 0x3A, 0x34, 0x70, 0xAF, 0x3E, 0xFF, 0x1F};
const uint8_t spTHREE[] PROGMEM = {0x0C, 0xE8, 0x2E, 0x94, 0x01, 0x4D, 0xBA, 0x4A, 0x40, 0x03, 0x16, 0x68, 0x69, 0x36, 0x1C, 0xE9, 0xBA, 0xB8, 0xE5, 0x39, 0x70, 0x72, 0x84, 0xDB, 0x51, 0xA4, 0xA8, 0x4E, 0xA3, 0xC9, 0x77, 0xB1, 0xCA, 0xD6, 0x52, 0xA8, 0x71, 0xED, 0x2A, 0x7B, 0x4B, 0xA6, 0xE0, 0x37, 0xB7, 0x5A, 0xDD, 0x48, 0x8E, 0x94, 0xF1, 0x64, 0xCE, 0x6D, 0x19, 0x55, 0x91, 0xBC, 0x6E, 0xD7, 0xAD, 0x1E, 0xF5, 0xAA, 0x77, 0x7A, 0xC6, 0x70, 0x22, 0xCD, 0xC7, 0xF9, 0x89, 0xCF, 0xFF, 0x03};
const uint8_t spFOUR[] PROGMEM = {0x08, 0x68, 0x21, 0x0D, 0x03, 0x04, 0x28, 0xCE, 0x92, 0x03, 0x23, 0x4A, 0xCA, 0xA6, 0x1C, 0xDA, 0xAD, 0xB4, 0x70, 0xED, 0x19, 0x64, 0xB7, 0xD3, 0x91, 0x45, 0x51, 0x35, 0x89, 0xEA, 0x66, 0xDE, 0xEA, 0xE0, 0xAB, 0xD3, 0x29, 0x4F, 0x1F, 0xFA, 0x52, 0xF6, 0x90, 0x52, 0x3B, 0x25, 0x7F, 0xDD, 0xCB, 0x9D, 0x72, 0x72, 0x8C, 0x79, 0xCB, 0x6F, 0xFA, 0xD2, 0x10, 0x9E, 0xB4, 0x2C, 0xE1, 0x4F, 0x25, 0x70, 0x3A, 0xDC, 0xBA, 0x2F, 0x6F, 0xC1, 0x75, 0xCB, 0xF2, 0xFF};
const uint8_t spFIVE[] PROGMEM = {0x08, 0x68, 0x4E, 0x9D, 0x02, 0x1C, 0x60, 0xC0, 0x8C, 0x69, 0x12, 0xB0, 0xC0, 0x28, 0xAB, 0x8C, 0x9C, 0xC0, 0x2D, 0xBB, 0x38, 0x79, 0x31, 0x15, 0xA3, 0xB6, 0xE4, 0x16, 0xB7, 0xDC, 0xF5, 0x6E, 0x57, 0xDF, 0x54, 0x5B, 0x85, 0xBE, 0xD9, 0xE3, 0x5C, 0xC6, 0xD6, 0x6D, 0xB1, 0xA5, 0xBF, 0x99, 0x5B, 0x3B, 0x5A, 0x30, 0x09, 0xAF, 0x2F, 0xED, 0xEC, 0x31, 0xC4, 0x5C, 0xBE, 0xD6, 0x33, 0xDD, 0xAD, 0x88, 0x87, 0xE2, 0xD2, 0xF2, 0xF4, 0xE0, 0x16, 0x2A, 0xB2, 0xE3, 0x63, 0x1F, 0xF9, 0xF0, 0xE7, 0xFF, 0x01};
const uint8_t spSIX[] PROGMEM = {0x04, 0xF8, 0xAD, 0x4C, 0x02, 0x16, 0xB0, 0x80, 0x06, 0x56, 0x35, 0x5D, 0xA8, 0x2A, 0x6D, 0xB9, 0xCD, 0x69, 0xBB, 0x2B, 0x55, 0xB5, 0x2D, 0xB7, 0xDB, 0xFD, 0x9C, 0x0D, 0xD8, 0x32, 0x8A, 0x7B, 0xBC, 0x02, 0x00, 0x03, 0x0C, 0xB1, 0x2E, 0x80, 0xDF, 0xD2, 0x35, 0x20, 0x01, 0x0E, 0x60, 0xE0, 0xFF, 0x01};
const uint8_t spSEVEN[] PROGMEM = {0x0C, 0xF8, 0x5E, 0x4C, 0x01, 0xBF, 0x95, 0x7B, 0xC0, 0x02, 0x16, 0xB0, 0xC0, 0xC8, 0xBA, 0x36, 0x4D, 0xB7, 0x27, 0x37, 0xBB, 0xC5, 0x29, 0xBA, 0x71, 0x6D, 0xB7, 0xB5, 0xAB, 0xA8, 0xCE, 0xBD, 0xD4, 0xDE, 0xA6, 0xB2, 0x5A, 0xB1, 0x34, 0x6A, 0x1D, 0xA7, 0x35, 0x37, 0xE5, 0x5A, 0xAE, 0x6B, 0xEE, 0xD2, 0xB6, 0x26, 0x4C, 0x37, 0xF5, 0x4D, 0xB9, 0x9A, 0x34, 0x39, 0xB7, 0xC6, 0xE1, 0x1E, 0x81, 0xD8, 0xA2, 0xEC, 0xE6, 0xC7, 0x7F, 0xFE, 0xFB, 0x7F};
const uint8_t spEIGHT[] PROGMEM = {0x65, 0x69, 0x89, 0xC5, 0x73, 0x66, 0xDF, 0xE9, 0x8C, 0x33, 0x0E, 0x41, 0xC6, 0xEA, 0x5B, 0xEF, 0x7A, 0xF5, 0x33, 0x25, 0x50, 0xE5, 0xEA, 0x39, 0xD7, 0xC5, 0x6E, 0x08, 0x14, 0xC1, 0xDD, 0x45, 0x64, 0x03, 0x00, 0x80, 0x00, 0xAE, 0x70, 0x33, 0xC0, 0x73, 0x33, 0x1A, 0x10, 0x40, 0x8F, 0x2B, 0x14, 0xF8, 0x7F};
const uint8_t spNINE[] PROGMEM = {0xE6, 0xA8, 0x1A, 0x35, 0x5D, 0xD6, 0x9A, 0x35, 0x4B, 0x8C, 0x4E, 0x6B, 0x1A, 0xD6, 0xA6, 0x51, 0xB2, 0xB5, 0xEE, 0x58, 0x9A, 0x13, 0x4F, 0xB5, 0x35, 0x67, 0x68, 0x26, 0x3D, 0x4D, 0x97, 0x9C, 0xBE, 0xC9, 0x75, 0x2F, 0x6D, 0x7B, 0xBB, 0x5B, 0xDF, 0xFA, 0x36, 0xA7, 0xEF, 0xBA, 0x25, 0xDA, 0x16, 0xDF, 0x69, 0xAC, 0x23, 0x05, 0x45, 0xF9, 0xAC, 0xB9, 0x8F, 0xA3, 0x97, 0x20, 0x73, 0x9F, 0x54, 0xCE, 0x1E, 0x45, 0xC2, 0xA2, 0x4E, 0x3E, 0xD3, 0xD5, 0x3D, 0xB1, 0x79, 0x24, 0x0D, 0xD7, 0x48, 0x4C, 0x6E, 0xE1, 0x2C, 0xDE, 0xFF, 0x0F};
const uint8_t spTEN[] PROGMEM = {0x0E, 0x38, 0x3C, 0x2D, 0x00, 0x5F, 0xB6, 0x19, 0x60, 0xA8, 0x90, 0x93, 0x36, 0x2B, 0xE2, 0x99, 0xB3, 0x4E, 0xD9, 0x7D, 0x89, 0x85, 0x2F, 0xBE, 0xD5, 0xAD, 0x4F, 0x3F, 0x64, 0xAB, 0xA4, 0x3E, 0xBA, 0xD3, 0x59, 0x9A, 0x2E, 0x75, 0xD5, 0x39, 0x6D, 0x6B, 0x0A, 0x2D, 0x3C, 0xEC, 0xE5, 0xDD, 0x1F, 0xFE, 0xB0, 0xE7, 0xFF, 0x03};
const uint8_t spELEVEN[] PROGMEM = {0xA5, 0xEF, 0xD6, 0x50, 0x3B, 0x67, 0x8F, 0xB9, 0x3B, 0x23, 0x49, 0x7F, 0x33, 0x87, 0x31, 0x0C, 0xE9, 0x22, 0x49, 0x7D, 0x56, 0xDF, 0x69, 0xAA, 0x39, 0x6D, 0x59, 0xDD, 0x82, 0x56, 0x92, 0xDA, 0xE5, 0x74, 0x9D, 0xA7, 0xA6, 0xD3, 0x9A, 0x53, 0x37, 0x99, 0x56, 0xA6, 0x6F, 0x4F, 0x59, 0x9D, 0x7B, 0x89, 0x2F, 0xDD, 0xC5, 0x28, 0xAA, 0x15, 0x4B, 0xA3, 0xD6, 0xAE, 0x8C, 0x8A, 0xAD, 0x54, 0x3B, 0xA7, 0xA9, 0x3B, 0xB3, 0x54, 0x5D, 0x33, 0xE6, 0xA6, 0x5C, 0xCB, 0x75, 0xCD, 0x5E, 0xC6, 0xDA, 0xA4, 0xCA, 0xB9, 0x35, 0xAE, 0x67, 0xB8, 0x46, 0x40, 0xB6, 0x28, 0xBB, 0xF1, 0xF6, 0xB7, 0xB9, 0x47, 0x20, 0xB6, 0x28, 0xBB, 0xFF, 0x0F};
const uint8_t spTWELVE[] PROGMEM = {0x09, 0x98, 0xDA, 0x22, 0x01, 0x37, 0x78, 0x1A, 0x20, 0x85, 0xD1, 0x50, 0x3A, 0x33, 0x11, 0x81, 0x5D, 0x5B, 0x95, 0xD4, 0x44, 0x04, 0x76, 0x9D, 0xD5, 0xA9, 0x3A, 0xAB, 0xF0, 0xA1, 0x3E, 0xB7, 0xBA, 0xD5, 0xA9, 0x2B, 0xEB, 0xCC, 0xA0, 0x3E, 0xB7, 0xBD, 0xC3, 0x5A, 0x3B, 0xC8, 0x69, 0x67, 0xBD, 0xFB, 0xE8, 0x67, 0xBF, 0xCA, 0x9D, 0xE9, 0x74, 0x08, 0xE7, 0xCE, 0x77, 0x78, 0x06, 0x89, 0x32, 0x57, 0xD6, 0xF1, 0xF1, 0x8F, 0x7D, 0xFE, 0x1F};
const uint8_t spTHIR_[] PROGMEM = {0x04, 0xA8, 0xBE, 0x5C, 0x00, 0xDD, 0xA5, 0x11, 0xA0, 0xFA, 0x72, 0x02, 0x74, 0x97, 0xC6, 0x01, 0x09, 0x9C, 0xA6, 0xAB, 0x30, 0x0D, 0xCE, 0x7A, 0xEA, 0x6A, 0x4A, 0x39, 0x35, 0xFB, 0xAA, 0x8B, 0x1B, 0xC6, 0x76, 0xF7, 0xAB, 0x2E, 0x79, 0x19, 0xCA, 0xD5, 0xEF, 0xCA, 0x57, 0x08, 0x14, 0xA1, 0xDC, 0x45, 0x64, 0x03, 0x00, 0xC0, 0xFF, 0x03};
const uint8_t spFIF_[] PROGMEM = {0x08, 0x98, 0x31, 0x93, 0x02, 0x1C, 0xE0, 0x80, 0x07, 0x5A, 0xD6, 0x1C, 0x6B, 0x78, 0x2E, 0xBD, 0xE5, 0x2D, 0x4F, 0xDD, 0xAD, 0xAB, 0xAA, 0x6D, 0xC9, 0x23, 0x02, 0x56, 0x4C, 0x93, 0x00, 0x05, 0x10, 0x90, 0x89, 0x31, 0xFC, 0x3F};
const uint8_t sp_TEEN[] PROGMEM = {0x09, 0x58, 0x2A, 0x25, 0x00, 0xCB, 0x9F, 0x95, 0x6C, 0x14, 0x21, 0x89, 0xA9, 0x78, 0xB3, 0x5B, 0xEC, 0xBA, 0xB5, 0x23, 0x13, 0x46, 0x97, 0x99, 0x3E, 0xD6, 0xB9, 0x2E, 0x79, 0xC9, 0x5B, 0xD8, 0x47, 0x41, 0x53, 0x1F, 0xC7, 0xE1, 0x9C, 0x85, 0x54, 0x22, 0xEC, 0xFA, 0xDB, 0xDD, 0x23, 0x93, 0x49, 0xB8, 0xE6, 0x78, 0xFF, 0x3F};
const uint8_t spTWENTY[] PROGMEM = {0x0A, 0xE8, 0x4A, 0xCD, 0x01, 0xDB, 0xB9, 0x33, 0xC0, 0xA6, 0x54, 0x0C, 0xA4, 0x34, 0xD9, 0xF2, 0x0A, 0x6C, 0xBB, 0xB3, 0x53, 0x0E, 0x5D, 0xA6, 0x25, 0x9B, 0x6F, 0x75, 0xCA, 0x61, 0x52, 0xDC, 0x74, 0x49, 0xA9, 0x8A, 0xC4, 0x76, 0x4D, 0xD7, 0xB1, 0x76, 0xC0, 0x55, 0xA6, 0x65, 0xD8, 0x26, 0x99, 0x5C, 0x56, 0xAD, 0xB9, 0x25, 0x23, 0xD5, 0x7C, 0x32, 0x96, 0xE9, 0x9B, 0x20, 0x7D, 0xCB, 0x3C, 0xFA, 0x55, 0xAE, 0x99, 0x1A, 0x30, 0xFC, 0x4B, 0x3C, 0xFF, 0x1F};
const uint8_t spHUNDRED[] PROGMEM = {0x04, 0xC8, 0x7E, 0x5C, 0x02, 0x0A, 0xA8, 0x62, 0x43, 0x03, 0xA7, 0xA8, 0x62, 0x43, 0x4B, 0x97, 0xDC, 0xF2, 0x14, 0xC5, 0xA7, 0x9B, 0x7A, 0xD3, 0x95, 0x37, 0xC3, 0x1E, 0x16, 0x4A, 0x66, 0x36, 0xF3, 0x5A, 0x89, 0x6E, 0xD4, 0x30, 0x55, 0xB5, 0x32, 0xB7, 0x31, 0xB5, 0xC1, 0x69, 0x2C, 0xE9, 0xF7, 0xBC, 0x96, 0x12, 0x39, 0xD4, 0xB5, 0xFD, 0xDA, 0x9B, 0x0F, 0xD1, 0x90, 0xEE, 0xF5, 0xE4, 0x17, 0x02, 0x45, 0x28, 0x77, 0x11, 0xD9, 0x40, 0x9E, 0x45, 0xDD, 0x2B, 0x33, 0x71, 0x7A, 0xBA, 0x0B, 0x13, 0x95, 0x2D, 0xF9, 0xF9, 0x7F};
const uint8_t spTHOUSAND[] PROGMEM = {0x0C, 0xE8, 0x2E, 0xD4, 0x02, 0x06, 0x98, 0xD2, 0x55, 0x03, 0x16, 0x68, 0x7D, 0x17, 0xE9, 0x6E, 0xBC, 0x65, 0x8C, 0x45, 0x6D, 0xA6, 0xE9, 0x96, 0xDD, 0xDE, 0xF6, 0xB6, 0xB7, 0x5E, 0x75, 0xD4, 0x93, 0xA5, 0x9C, 0x7B, 0x57, 0xB3, 0x6E, 0x7D, 0x12, 0x19, 0xAD, 0xDC, 0x29, 0x8D, 0x4F, 0x93, 0xB4, 0x87, 0xD2, 0xB6, 0xFC, 0xDD, 0xAC, 0x22, 0x56, 0x02, 0x70, 0x18, 0xCA, 0x18, 0x26, 0xB5, 0x90, 0xD4, 0xDE, 0x6B, 0x29, 0xDA, 0x2D, 0x25, 0x17, 0x8D, 0x79, 0x88, 0xD4, 0x48, 0x79, 0x5D, 0xF7, 0x74, 0x75, 0xA1, 0x94, 0xA9, 0xD1, 0xF2, 0xED, 0x9E, 0xAA, 0x51, 0xA6, 0xD4, 0x9E, 0x7F, 0xED, 0x6F, 0xFE, 0x2B, 0xD1, 0xC7, 0x3D, 0x89, 0xFA, 0xB7, 0x0D, 0x57, 0xD3, 0xB4, 0xF5, 0x37, 0x55, 0x37, 0x2E, 0xE6, 0xB2, 0xD7, 0x57, 0xFF, 0x0F};
const uint8_t spT[] PROGMEM = {0x01, 0xD8, 0xB6, 0xDD, 0x01, 0x2F, 0xF4, 0x38, 0x60, 0xD5, 0xD1, 0x91, 0x4D, 0x97, 0x84, 0xE6, 0x4B, 0x4E, 0x36, 0xB2, 0x10, 0x67, 0xCD, 0x19, 0xD9, 0x2C, 0x01, 0x94, 0xF1, 0x78, 0x66, 0x33, 0xEB, 0x79, 0xAF, 0x7B, 0x57, 0x87, 0x36, 0xAF, 0x52, 0x08, 0x9E, 0x6B, 0xEA, 0x5A, 0xB7, 0x7A, 0x94, 0x73, 0x45, 0x47, 0xAC, 0x5A, 0x9C, 0xAF, 0xFF, 0x07};
const uint8_t spAND[] PROGMEM = {0xA9, 0x6B, 0x21, 0xB9, 0x22, 0x66, 0x9F, 0xAE, 0xC7, 0xE1, 0x70, 0x7B, 0x72, 0xBB, 0x5B, 0xDF, 0xEA, 0x56, 0xBB, 0x5C, 0x65, 0xCB, 0x66, 0xC5, 0x3D, 0x67, 0xD7, 0xAB, 0x6D, 0x2E, 0x64, 0x30, 0x93, 0xEE, 0xB1, 0xCD, 0x3D, 0x92, 0xB9, 0x9A, 0xDA, 0xB2, 0x8E, 0x40, 0x12, 0x9A, 0x6A, 0xEB, 0x96, 0x8F, 0x78, 0x98, 0xB3, 0x2A, 0xB4, 0xD3, 0x48, 0xAA, 0x2F, 0x7D, 0xA7, 0x7B, 0xFB, 0x0C, 0x73, 0x71, 0x5C, 0xCE, 0x6E, 0x5C, 0x52, 0x6C, 0x73, 0x79, 0x9A, 0x13, 0x4B, 0x89, 0x45, 0xE9, 0x6E, 0x49, 0x42, 0xA9, 0x57, 0xFF, 0x3F};
const uint8_t spMINUS[] PROGMEM = {0xE6, 0x28, 0xC4, 0xF8, 0x44, 0x9A, 0xFB, 0xCD, 0xAD, 0x8D, 0x2A, 0x4E, 0x4A, 0xBC, 0xB8, 0x8C, 0xB9, 0x8A, 0xA9, 0x48, 0xED, 0x72, 0x87, 0xD3, 0x74, 0x3B, 0x1A, 0xA9, 0x9D, 0x6F, 0xB3, 0xCA, 0x5E, 0x8C, 0xC3, 0x7B, 0xF2, 0xCE, 0x5A, 0x5E, 0x35, 0x66, 0x5A, 0x3A, 0xAE, 0x55, 0xEB, 0x9A, 0x57, 0x75, 0xA9, 0x29, 0x6B, 0xEE, 0xB6, 0xD5, 0x4D, 0x37, 0xEF, 0xB5, 0x5D, 0xC5, 0x95, 0x84, 0xE5, 0xA6, 0xFC, 0x30, 0xE0, 0x97, 0x0C, 0x0D, 0x58, 0x40, 0x03, 0x1C, 0xA0, 0xC0, 0xFF, 0x03};
const uint8_t spMILLI[] PROGMEM = {0x6E, 0xF0, 0x8A, 0xB3, 0x4B, 0xEB, 0xC6, 0xAE, 0x36, 0xA7, 0x1A, 0x3A, 0x54, 0x53, 0xD6, 0xDC, 0xEC, 0x66, 0x23, 0xDF, 0x58, 0x26, 0x43, 0xB4, 0xCD, 0xEA, 0x74, 0x5D, 0x94, 0x46, 0xF0, 0x96, 0x3B, 0x9D, 0x79, 0x98, 0x26, 0x75, 0xDB, 0xB3, 0xD7, 0xB6, 0xF5, 0x90, 0xA8, 0x91, 0x9F, 0xEA, 0x9E, 0xEE, 0xE9, 0x9B, 0x20, 0x7D, 0xCB, 0xFF, 0x03};
const uint8_t spMETER[] PROGMEM = {0xA1, 0x8F, 0x5C, 0xB5, 0x56, 0x92, 0xE4, 0xE1, 0xF4, 0xDD, 0x0B, 0x59, 0x6B, 0xE3, 0x53, 0x8C, 0x14, 0x44, 0x15, 0x8B, 0x46, 0x3A, 0xB3, 0x03, 0x7B, 0xBE, 0x99, 0x89, 0x49, 0xB7, 0x72, 0xC4, 0xEA, 0x4C, 0x01, 0xD8, 0x2E, 0xC8, 0x03, 0xA3, 0xAB, 0x91, 0x39, 0x2C, 0x17, 0x8D, 0xAE, 0x36, 0xE6, 0x34, 0x7F, 0x3D, 0xE6, 0xEA, 0x13, 0x6C, 0x79, 0x73, 0x3B, 0xAA, 0x1B, 0xB0, 0xD3, 0x3C, 0xFD, 0x6A, 0x4F, 0xF1, 0x09, 0x35, 0x9E, 0xA5, 0xBE, 0xFF, 0x0F};
const uint8_t spADJUST[] PROGMEM = {0xAD, 0xAD, 0xA1, 0xD5, 0xC4, 0x5A, 0x9F, 0xB1, 0xFA, 0x14, 0xB3, 0x78, 0xBC, 0x87, 0x31, 0x55, 0x9B, 0xEC, 0xC2, 0x6B, 0xC4, 0xE6, 0xB9, 0xDB, 0xB8, 0x97, 0x24, 0x87, 0xA6, 0x99, 0x59, 0x61, 0x4B, 0x1C, 0x05, 0x63, 0x56, 0x79, 0x6C, 0x05, 0x4C, 0xC5, 0x14, 0x81, 0x35, 0xB4, 0x98, 0xAC, 0xAE, 0x7D, 0x6E, 0x77, 0xAA, 0xE2, 0xD2, 0x5A, 0x63, 0xD5, 0xAD, 0x6E, 0xBD, 0xBA, 0xE2, 0xD3, 0x8A, 0xAB, 0xF2, 0x1C, 0x15, 0x50, 0x41, 0x8A, 0x03, 0x7E, 0x29, 0xF1, 0x80, 0x05, 0x2C, 0xA0, 0x01, 0x01, 0xFC, 0xD6, 0x2A, 0x01, 0x60, 0xC0, 0x0B, 0xEC, 0x16, 0x60, 0x40, 0xB7, 0x63, 0xFF, 0x0F};
const uint8_t spDIRECTION[] PROGMEM = {0xA5, 0x7E, 0xBE, 0x3C, 0x49, 0x14, 0xAF, 0x6E, 0xAA, 0x52, 0x72, 0xCD, 0x77, 0xBA, 0x66, 0x4A, 0x38, 0xAC, 0xDB, 0xE9, 0x8A, 0x0F, 0xB6, 0xB0, 0xF4, 0xAD, 0x4B, 0x5D, 0xDC, 0x35, 0xED, 0xCF, 0xF6, 0xD4, 0xA5, 0x68, 0xB8, 0x85, 0xFB, 0x53, 0xD6, 0x90, 0x34, 0x1E, 0x9D, 0x6E, 0x31, 0xF2, 0x36, 0x9D, 0x4A, 0x6C, 0x91, 0xC9, 0x47, 0x18, 0x63, 0xD1, 0xD8, 0x02, 0xE8, 0xC1, 0xCC, 0x01, 0x63, 0x6C, 0x45, 0x20, 0x02, 0x1E, 0x68, 0x45, 0x8D, 0xAA, 0x6E, 0xD1, 0x69, 0x36, 0x63, 0x69, 0x81, 0x2D, 0x25, 0x9A, 0xD4, 0x23, 0x1D, 0x5D, 0x0B, 0xA5, 0x7B, 0xB4, 0x78, 0xF9, 0xDB, 0x7D, 0x23, 0x18, 0xB9, 0x58, 0x7C, 0xFF, 0xBB, 0xAF, 0x19, 0xC1, 0x54, 0x4B, 0xF6, 0xFF};
const uint8_t spGO[] PROGMEM = {0x06, 0x08, 0xDA, 0x75, 0xB5, 0x8D, 0x87, 0x4B, 0x4B, 0xBA, 0x5B, 0xDD, 0xE2, 0xE4, 0x49, 0x4E, 0xA6, 0x73, 0xBE, 0x9B, 0xEF, 0x62, 0x37, 0xBB, 0x9B, 0x4B, 0xDB, 0x82, 0x1A, 0x5F, 0xC1, 0x7C, 0x79, 0xF7, 0xA7, 0xBF, 0xFE, 0x1F};
const uint8_t spREADY[] PROGMEM = {0x6A, 0xB4, 0xD9, 0x25, 0x4A, 0xE5, 0xDB, 0xD9, 0x8D, 0xB1, 0xB2, 0x45, 0x9A, 0xF6, 0xD8, 0x9F, 0xAE, 0x26, 0xD7, 0x30, 0xED, 0x72, 0xDA, 0x9E, 0xCD, 0x9C, 0x6D, 0xC9, 0x6D, 0x76, 0xED, 0xFA, 0xE1, 0x93, 0x8D, 0xAD, 0x51, 0x1F, 0xC7, 0xD8, 0x13, 0x8B, 0x5A, 0x3F, 0x99, 0x4B, 0x39, 0x7A, 0x13, 0xE2, 0xE8, 0x3B, 0xF5, 0xCA, 0x77, 0x7E, 0xC2, 0xDB, 0x2B, 0x8A, 0xC7, 0xD6, 0xFA, 0x7F};
const uint8_t spTO[] PROGMEM = {0x09, 0xD8, 0x4E, 0x34, 0x00, 0x4B, 0xA7, 0xA5, 0xBC, 0xE9, 0x62, 0x55, 0x4B, 0x53, 0x9A, 0xAC, 0x5C, 0x2D, 0xF9, 0x4B, 0xE9, 0x32, 0x73, 0xD3, 0xA1, 0x3D, 0xA5, 0x4F, 0x52, 0x2D, 0x9B, 0xB6, 0xA4, 0x21, 0x79, 0x95, 0xC0, 0xAC, 0xAD, 0x16, 0xED, 0xDC, 0x22, 0x23, 0xC2, 0xFF, 0x03};
const uint8_t spTURN[] PROGMEM = {0x01, 0x18, 0xA9, 0xCC, 0x02, 0x06, 0x28, 0x4E, 0xA9, 0x14, 0x39, 0x25, 0x69, 0x4B, 0xBA, 0x5D, 0xAE, 0xAA, 0x84, 0x15, 0x5A, 0xF5, 0xBE, 0xAB, 0x59, 0xCF, 0x61, 0xCE, 0x7D, 0x6B, 0x5B, 0x09, 0x49, 0x76, 0xEE, 0xB5, 0x1E, 0xE5, 0x69, 0x2E, 0x44, 0xD3, 0x9A, 0xE6, 0x27, 0x7C, 0x4D, 0x09, 0xA5, 0x47, 0xDC, 0xF8, 0xB9, 0xAF, 0x7B, 0x62, 0xB7, 0x70, 0xE6, 0xBE, 0x1A, 0x54, 0x4C, 0xB8, 0xDD, 0xFF, 0x03};
const uint8_t spUNIT[] PROGMEM = {0x61, 0xB9, 0x96, 0x84, 0xB9, 0x56, 0xE5, 0xB9, 0xCE, 0x63, 0xDE, 0xCE, 0x0D, 0x30, 0x36, 0x9F, 0x6E, 0x86, 0x36, 0x60, 0xE9, 0x7B, 0xCA, 0x5E, 0x93, 0x45, 0xA4, 0xEB, 0xC9, 0xBB, 0x77, 0x72, 0xE7, 0x2D, 0x2B, 0xAB, 0xD6, 0x24, 0x94, 0x17, 0x8F, 0xA2, 0x79, 0x4C, 0xD5, 0x48, 0x5D, 0xAA, 0xEE, 0x21, 0x23, 0x42, 0xF1, 0x1A, 0x66, 0x54, 0x15, 0x97, 0xD6, 0x6B, 0x19, 0xD1, 0xC5, 0xC5, 0x77, 0xEF, 0xB3, 0x9F, 0x7E, 0x47, 0xA0, 0x08, 0xE5, 0x2E, 0x22, 0x1B, 0x00, 0x01, 0xCB, 0xBB, 0x3B, 0xE0, 0xD7, 0x0A, 0x05, 0x9C, 0xD0, 0x4D, 0x80, 0xE6, 0x92, 0xFE, 0x1F};
// Constants and variables
#define gY_a 0x1E // gyro IIC address ( 3C/3D for PIC W/R )
long Yv; // Y value of gyro compass
long sYv; // save area for Yva
float Yf; // Yv average (float)
// for 28BYJ-48 steppers
int TmStp = 3; // delay time per step (minimum 3)
#define rPi 498 // rotation count for 180 degrees
// for VL6180X ToF distance sensor
float Tot = 0; // ToF measurement total
float Ave = 0; // average of ToF measurement
#define sFar 255 // ToF response value (spec) if too-far
#define sPos 6 // sensor inside distance from front edge
int Imm = 0; // integer value of milli-meter
int dGo; // distance to go forward
int Far = sFar - sPos; // wall is far
int Near = 45; // wall is near (mm)
// IR-reflective obstacle-sensors
byte obsS; // obstacle status byte
#define RLok B00000011
#define Rok B00000001
#define Lok B00000010
#define RLng B00000000
// Rotation priority
#define Right 0 // rotate right
#define Left 1 // rotate left
int RLprty = Right; // Set initial priority right
int nRt = 0; // # of continuous right turns
int nLt = 0; // # of continuous left turns
#define tMax 3 // max # of cont. same-dir turns
// Hoko as current direction
#define HokoN 0 // north
#define HokoE 1 // east
#define HokoS 2 // south
#define HokoW 3 // west
int Hoko = HokoN; // Set initial Hoko = north
void setup() { // Arduino Setup *************
// for steppers
DDRD = B11110000; // RD4-7(D4-7) output for stpper-1
PORTD &= B00001111; // set all stepper-1 pins LOW
DDRB = B00001111; // RB0-3(D8-11) output for stepper-2
PORTB &= B11110000; // set all stepper-2 pins LOW
// for R/L obstacle sensors
pinMode(obsPwr, OUTPUT); // Power for obstacle sennsors
digitalWrite(obsPwr, LOW); // power-off
pinMode(obsR, INPUT_PULLUP); //right side obstacle sensor
pinMode(obsL, INPUT_PULLUP); //left side obstacle sensor
Serial.begin(9600);
bLed(1, 500, 500); // blink Led to show I am awake
Wire.begin();
// for ToF
tOF.init(); // initialize ToF
tOF.configureDefault();
tOF.setTimeout(50);
// for Gyro compass
// single measurement (wait>6mS i.e.: !dRdy unused)
pgY(); // initialize gyro
delay(200);
// initial turn to north *** for test
// ttN(); // *** for test
// delay(300);
}
void loop() { // Arduino Loop *************
// Measure distance to wall and set Imm
Distance();
// Serial.print("Imm=");
// Serial.println(Imm);
// 1)when wall is far away
if (Imm >= Far) {
dGo = (float)Imm * 1.8 - 30; // 1mm = 1.8xFwd (adjusted)
Fwd(dGo); // go forward
}
// 2) when wall is not far
if (Imm < Far) {
sayNum(Imm);
sPK.say(spMILLI);
sPK.say(spMETER);
delay(250);
// and if wall is very near, turn
if (Imm < Near) {
sPK.say(spTURN);
Turn();
} else { // not very near
dGo = (float)Imm * 1.8 - 30; // 1mm = 1.8xFwd (adjusted)
if (dGo > 0) { // if still okay
sPK.say(spGO);
Fwd(dGo); // go forward
} else { // if not okay
sPK.say(spTURN);
Turn();
} // end of else (not okay)
} // end of else (Imm >= Near)
} // end of if (Imm < Far)
} // end of main Loop
/********************************
* *** User defined functions ***
* *****************************/
// *** Measure distance to wall
void Distance(void) {
#define mcount 5
float Tot = 0;
for (int i = 0; i < mcount; i++) {
Tot = Tot + tOF.readRangeSingleMillimeters();
}
float Ave = (float) Tot / mcount;
Imm = Ave - sPos; // minus sensor position
}
// *** Blink Led
void bLed(int nTimes, int onLen, int offLen)
{
for (int m = 0; m < nTimes; m++) {
digitalWrite(Led, HIGH);
delay(onLen);
digitalWrite(Led, LOW);
delay(offLen);
}
}
// *** gyro compass funcions
// prepare gY(HMC5883L) = set control registers
void pgY(void) {
// Conf-reg A = B011(#samples=8)110(75Hz@Cont-mode)00(no bias)=0x78
// Conf-reg B = B000(±0.88Gauss)00000(must be cleared)=0x00
// Mode reg = B00000(must be cleared)01(single measurement)=0x01
Wire.beginTransmission(gY_a);
Wire.write(0x00); // point addr 0 (Conf-reg A)
Wire.write(0x78); // Conf-reg-A = 0x78
Wire.write(0x00); // Conf-reg-B = 0x00
Wire.endTransmission();
}
// read Y-axis value of gyro HMC5883L=GY271
void rgY(void) {
#define numadd 20 // number of additions for Yv average
long Yvt; // Yv total
Yvt = 0; // clear total
for (int j = 0; j < numadd; j++) {
Wire.beginTransmission(gY_a);
Wire.write(0x02); // point Mode-register
Wire.write(0x01); // set signle measm't mode
Wire.endTransmission();
delay(6); // wait>6mS for data update
Wire.requestFrom(gY_a, 6); // read six bytes
while (Wire.available() < 6) {} // wait for data ready
byte bXh = Wire.read(); // X high byte
byte bXl = Wire.read(); // X low byte
byte bZh = Wire.read(); // Z high byte
byte bZl = Wire.read(); // Z low byte
byte bYh = Wire.read(); // Y high byte
byte bYl = Wire.read(); // Y low byte
Wire.endTransmission();
Yv = bYl + bYh * 256; // Set Yv
Yvt = Yvt + Yv;
}
Yf = Yvt / numadd; // get average (float value)
Yv = Yf + 0.5; // set integer value
}
// *** Turn to appropriate direction
void Turn(void) {
obsChk(); // check obstacle sensors
switch (obsS) { // results in obsStatus
case 0: // when both directions NG
while (1) {
bLed(1, 100, 300); // blink Led
}
case 1: // when the right is OK & left is NG
TurnR();
break;
case 2: // when the left is OK & right is NG
TurnL();
break;
case 3: // when both directions are OK
if (RLprty == Right) {
if (nRt < tMax) {
TurnR();
} else {
RLprty = Left; // new priority is Left
TurnL();
}
} else {
if (nLt < tMax) {
TurnL();
} else {
RLprty = Right; // new priority is Right
TurnR();
}
}
}
if (Hoko == HokoN) ttN(); // if back to North, adjust direction
}
// *** Turn right
void TurnR(void) {
Tr(rPi / 2); // right turn 90 degrees
nLt = 0; // clear # of continuous Left turns
nRt ++; // inclease # continuous Right turns
Hoko++;
if (Hoko >= 4) Hoko = HokoN;
}
// *** Turn left
void TurnL(void) {
Tl(rPi / 2); // left turn 90 degrees
nRt = 0; // clear # of continuous Right turns
nLt ++; // inclease # continuous Left turns
Hoko--;
if (Hoko <= -1) Hoko = HokoW;
}
// *** Obstacle checking
void obsChk(void) {
obsS &= B00000000; // clear obsStatus
digitalWrite (obsPwr, HIGH); // sensor power on
delay (20); // wait for power stability
if (digitalRead(obsL) == HIGH)obsS |= B00000010;
if (digitalRead(obsR) == HIGH)obsS |= B00000001;
digitalWrite (obsPwr, LOW); // save power
}
// *** Adjusting direction to North
void ttN(void)
{
sPK.say(spADJUST);
delay(20);
sPK.say(spDIRECTION);
#define nNc 20 // number of adjustings ± North
float Ang = 2.55 * rPi / 180.0; // unit 2.55 degrees
int Fdeg = Ang; // convert Ang to integer Fdeg
Tl(Fdeg * nNc / 2); // turn left by Fdeg x nNc/2
float maxY = 0; // to set max Y
int Nchk = 0; // number of checks
int maxN = 0; // Number of checks when Y is max
for (Nchk = 1; Nchk <= nNc; Nchk++) {
Tr(Fdeg); // turn right by Fdeg
delay(100); // avoid mechanical vibration
rgY(); // get Y axis value
if (Yf >= maxY) {
maxY = Yf;
maxN = Nchk;
}
}
if (maxN == nNc)maxN--;
int mDeg = (nNc - maxN) * Fdeg;
Tl(mDeg);
delay(100);
sPK.say(spREADY);
}
// *** Stepper movement function list ***
// 1)Go forward, Backward :Fwd,Bwd
// 2)Turn right, Turn left: Tr,Tl
// *** Go forward with both steppers
void Fwd(int nTimes) {
for (int i = 0; i < nTimes; i++) {
// phase 31 PORTD=0011xxxx, PORTB=xxxx0011
PORTD &= B00111111; // clear bits
PORTD |= B00110000; // set bits
PORTB &= B11110011; // clear bits
PORTB |= B00000011; // set bits
delay(TmStp);
// phase 32 PORTD=0110xxxx, PORTB=xxxx0110
PORTD &= B01101111; // clear bits
PORTD |= B01100000; // set bits
PORTB &= B11110110; // clear bits
PORTB |= B00000110; // set bits
delay(TmStp);
// phase 33 PORTD=1100xxxx, PORTB=xxxx1100
PORTD &= B11001111; // clear bits
PORTD |= B11000000; // set bits
PORTB &= B11111100; // clear bits
PORTB |= B00001100; // set bits
delay(TmStp);
// phase 34 PORTD=1001xxxx, PORTB=xxxx1001
PORTD &= B10011111; // clear bits
PORTD |= B10010000; // set bits
PORTB &= B11111001; // clear bits
PORTB |= B00001001; // set bits
delay(TmStp);
}
// return to phase 31 PORTD=0011xxxx, PORTB=xxxx0011
PORTD &= B00111111; // clear bits
PORTD |= B00110000; // set bits
PORTB &= B11110011; // clear bits
PORTB |= B00000011; // set bits
delay(TmStp);
Stp();
}
// *** Go backward with both steppers
void Bwd(int nTimes) {
for (int i = 0; i < nTimes; i++) {
// phase 34 PORTD=1001xxxx, PORTB=xxxx1001
PORTD &= B10011111; // clear bits
PORTD |= B10010000; // set bits
PORTB &= B11111001; // clear bits
PORTB |= B00001001; // set bits
delay(TmStp);
// phase 33 PORTD=1100xxxx, PORTB=xxxx1100
PORTD &= B11001111; // clear bits
PORTD |= B11000000; // set bits
PORTB &= B11111100; // clear bits
PORTB |= B00001100; // set bits
delay(TmStp);
// phase 32 PORTD=0110xxxx, PORTB=xxxx0110
PORTD &= B01101111; // clear bits
PORTD |= B01100000; // set bits
PORTB &= B11110110; // clear bits
PORTB |= B00000110; // set bits
delay(TmStp);
// phase 31 PORTD=0011xxxx, PORTB=xxxx0011
PORTD &= B00111111; // clear bits
PORTD |= B00110000; // set bits
PORTB &= B11110011; // clear bits
PORTB |= B00000011; // set bits
delay(TmStp);
}
// return to phase 34 PORTD=1001xxxx, PORTB=xxxx1001
PORTD &= B10011111; // clear bits
PORTD |= B10010000; // set bits
PORTB &= B11111001; // clear bits
PORTB |= B00001001; // set bits
delay(TmStp);
Stp();
}
// *** Turn left
void Tl(int nTimes) {
for (int i = 0; i < nTimes; i++) {
// phase 41 PORTD=0011xxxx, PORTB=xxxx1001
PORTD &= B00111111; // clear bits
PORTD |= B00110000; // set bits
PORTB &= B11111001; // clear bits
PORTB |= B00001001; // set bits
delay(TmStp);
// phase 42 PORTD=0110xxxx, PORTB=xxxx1100
PORTD &= B01101111; // clear bits
PORTD |= B01100000; // set bits
PORTB &= B11111100; // clear bits
PORTB |= B00001100; // set bits
delay(TmStp);
// phase 43 PORTD=1100xxxx, PORTB=xxxx0110
PORTD &= B11001111; // clear bits
PORTD |= B11000000; // set bits
PORTB &= B11110110; // clear bits
PORTB |= B00000110; // set bits
delay(TmStp);
// phase 44 PORTD=1001xxxx, PORTB=xxxx0011
PORTD &= B10011111; // clear bits
PORTD |= B10010000; // set bits
PORTB &= B11110011; // clear bits
PORTB |= B00000011; // set bits
delay(TmStp);
}
// return to phase 41 PORTD=0011xxxx, PORTB=xxxx1001
PORTD &= B00111111; // clear bits
PORTD |= B00110000; // set bits
PORTB &= B11111001; // clear bits
PORTB |= B00001001; // set bits
delay(TmStp);
Stp();
}
// *** Turn right
void Tr(int nTimes) {
for (int i = 0; i < nTimes; i++) {
// phase 44 PORTD=1001xxxx, PORTB=xxxx0011
PORTD &= B10011111; // clear bits
PORTD |= B10010000; // set bits
PORTB &= B11110011; // clear bits
PORTB |= B00000011; // set bits
delay(TmStp);
// phase 43 PORTD=1100xxxx, PORTB=xxxx0110
PORTD &= B11001111; // clear bits
PORTD |= B11000000; // set bits
PORTB &= B11110110; // clear bits
PORTB |= B00000110; // set bits
delay(TmStp);
// phase 42 PORTD=0110xxxx, PORTB=xxxx1100
PORTD &= B01101111; // clear bits
PORTD |= B01100000; // set bits
PORTB &= B11111100; // clear bits
PORTB |= B00001100; // set bits
delay(TmStp);
// phase 41 PORTD=0011xxxx, PORTB=xxxx1001
PORTD &= B00111111; // clear bits
PORTD |= B00110000; // set bits
PORTB &= B11111001; // clear bits
PORTB |= B00001001; // set bits
delay(TmStp);
}
// return to phase 44 PORTD=1001xxxx, PORTB=xxxx0011
PORTD &= B10011111; // clear bits
PORTD |= B10010000; // set bits
PORTB &= B11110011; // clear bits
PORTB |= B00000011; // set bits
delay(TmStp);
Stp();
}
// *** Stop both steppers
void Stp(void) {
// PORTB=xxxx0000, PORTD=0000xxxx
PORTB &= B11110000;
PORTD &= B00001111;
delay(TmStp);
}
// *** Say number from-999,999 to 999,999
void sayNum(long n) {
if (n < 0) {
sPK.say(spMINUS);
sayNum(-n);
} else if (n == 0) {
sPK.say(spZERO);
} else {
if (n >= 1000) {
int thousands = n / 1000;
sayNum(thousands);
sPK.say(spTHOUSAND);
n %= 1000;
if ((n > 0) && (n < 100)) sPK.say(spAND);
}
if (n >= 100) {
int hundreds = n / 100;
sayNum(hundreds);
sPK.say(spHUNDRED);
n %= 100;
if (n > 0) sPK.say(spAND);
}
if (n > 19) {
int tens = n / 10;
switch (tens) {
case 2: sPK.say(spTWENTY); break;
case 3: sPK.say(spTHIR_); sPK.say(spT); break;
case 4: sPK.say(spFOUR); sPK.say(spT); break;
case 5: sPK.say(spFIF_); sPK.say(spT); break;
case 6: sPK.say(spSIX); sPK.say(spT); break;
case 7: sPK.say(spSEVEN); sPK.say(spT); break;
case 8: sPK.say(spEIGHT); sPK.say(spT); break;
case 9: sPK.say(spNINE); sPK.say(spT); break;
}
n %= 10;
}
switch (n) {
case 1: sPK.say(spONE); break;
case 2: sPK.say(spTWO); break;
case 3: sPK.say(spTHREE); break;
case 4: sPK.say(spFOUR); break;
case 5: sPK.say(spFIVE); break;
case 6: sPK.say(spSIX); break;
case 7: sPK.say(spSEVEN); break;
case 8: sPK.say(spEIGHT); break;
case 9: sPK.say(spNINE); break;
case 10: sPK.say(spTEN); break;
case 11: sPK.say(spELEVEN); break;
case 12: sPK.say(spTWELVE); break;
case 13: sPK.say(spTHIR_); sPK.say(sp_TEEN); break;
case 14: sPK.say(spFOUR); sPK.say(sp_TEEN); break;
case 15: sPK.say(spFIF_); sPK.say(sp_TEEN); break;
case 16: sPK.say(spSIX); sPK.say(sp_TEEN); break;
case 17: sPK.say(spSEVEN); sPK.say(sp_TEEN); break;
case 18: sPK.say(spEIGHT); sPK.say(sp_TEEN); break;
case 19: sPK.say(spNINE); sPK.say(sp_TEEN); break;
}
}
}
// end of program
上のほうにある#include文3つの<>内が、このブログへの表示方法（PreおよびCodeタグの使用）では消えてしまうようなので、右側にコメントをいれました。
今後何か気づくたびに記事とプログラムは更新させていただくことがありますが、ご容赦ください。
では今回はこのへんで。
©2019 Akira Tominaga, All rights reserved.
