2015/10/13
加速度センサ(ADXL345)を使ったWi-Fiラジコン・コントローラ
加速度センサ(ADXL345)を使ったWi-Fiラジコンカーのコントローラを作りました。
スマホを使えない幼児向けのコントローラになります。
ESP-WROOM-02にI2Cで3軸加速度センサ(ADXL345)を接続します。
コントローラからWi-Fi AP(SoftAP)モードのラジコンカーに接続し、UDPパケットで左右のモータの速度制御データを送信します。
UDPパケットをiOSCの形式に合わせることでラジコンカー側の変更を不要にします。
このコントローラをおもちゃの「魔法の杖」とか「おきあがりこぼし」などに組込めば面白いかと・・・(^-^;
これを使って操作してみました。
送信機ソースコード(main)
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 |
#include <ESP8266WiFi.h> const char ssid[] = "ESPCar"; const char pass[] = "ESP8266ap"; #include <WiFiUdp.h> WiFiUDP udp; IPAddress carServerIP(192, 168, 4, 1); const int carPort = 10000; #include <Ticker.h> #define STATUS_LED 13 int Blink_val = -2048, Blink_inc = 0, Blink_add = 0; Ticker Ticks_100, Blink_led; unsigned long ticks0 = 0, ticks1 = 0, ticks2 = 0; void ticks100() { if (ticks0) --ticks0; ticks1++; } void blinkLED() { if (Blink_add < 0) return; if (Blink_add < 2) { analogWrite(STATUS_LED, ((Blink_add == 1) ? 1023 : 0)); return; } if (Blink_inc == 1) Blink_val += Blink_add; else Blink_val -= Blink_add; if (Blink_val <= 0) Blink_inc = 1, Blink_val = 0; if (Blink_val >= 1000) Blink_inc = 0; analogWrite(STATUS_LED, Blink_val); } void setBlink(int n) { if (Blink_val != -2048) Blink_led.detach(); Blink_val = 0; Blink_inc = 1; Blink_add = n; Blink_led.attach(0.01, blinkLED); } #define POWER_PIN 12 void powerON() { setBlink(1); digitalWrite(POWER_PIN, HIGH); } void powerOFF() { setBlink(0); digitalWrite(POWER_PIN, LOW); while(1) delay(100); } void sendUDPpacket(byte *buff, size_t plen) { udp.beginPacket(carServerIP, carPort); udp.write(buff, plen); udp.endPacket(); } byte sendBuff[32] = "/osc/\0 ,i \0\0\0\0"; int loopc = 10, pll, prr, phl, phn; void sendCar(int ll, int rr, int hl, int hn) { if (++loopc > 10) { pll = prr = phl = phn = -1; loopc = 0; } if (pll != ll) { sendBuff[ 5] = 'L'; sendBuff[15] = pll = ll; sendUDPpacket(sendBuff, 16); } if (prr != rr) { sendBuff[ 5] = 'R'; sendBuff[15] = prr = rr; sendUDPpacket(sendBuff, 16); } if (phl != hl) { sendBuff[ 5] = 'F'; sendBuff[15] = phl = hl; sendUDPpacket(sendBuff, 16); } if (phn != hn) { sendBuff[ 5] = 'C'; sendBuff[15] = phn = hn; sendUDPpacket(sendBuff, 16); } } void setup() { pinMode(STATUS_LED, OUTPUT); pinMode(POWER_PIN, OUTPUT); powerON(); Serial.begin(115200); Serial.printf("\nStart\n"); Serial.printf("\nADXL345 "); if (ADXL345_Init() != 0) { Serial.println("NG"); powerOFF(); } Serial.println("OK"); Serial.print("Connecting to "); Serial.println(ssid); WiFi.begin(ssid, pass); for (int i = 0; (WiFi.status() != WL_CONNECTED); i++) { if (i >= 40) { Serial.println("Wi-Fi Car NG"); powerOFF(); } Serial.print("."); delay(500); } Serial.print("\nWiFi connected\n"); Serial.print("IP address: "); Serial.println(WiFi.localIP()); Serial.println("Starting UDP"); udp.begin(carPort); Serial.print("Local port: "); Serial.println(udp.localPort()); } const int HomeXmin = - 8, HomeXmax = 8; const int HomeYmin = - 8, HomeYmax = 8; const int RangXmin = -64, RangXmax = 63; const int RangYmin = -64, RangYmax = 63; void loop() { int nX, nY, nZ; int pstat = 9, stat = 0, homec = 0, holdc = 0; int vL = 0, vR = 0, HL = 0, HN = 0; int pX = -1, pY = -1, wX, wY; Ticks_100.attach(0.1, ticks100); setBlink(10); loopc = 10; while (1) { if (pstat != stat) { pstat = stat; Serial.printf("stat=%d\n",stat); } ADXL345_readXYZ(&nX, &nY, &nZ); // Serial.printf("%3d %3d ", nX, nY); nX /= 2; nY /= 2; if (nX < RangXmin) nX = RangXmin; if (nY < RangYmin) nY = RangYmin; if (nX > RangXmax) nX = RangXmax; if (nY > RangYmax) nY = RangYmax; if ((nX > HomeXmin) && (nX < HomeXmax)) nX = 0; if ((nY > HomeYmin) && (nY < HomeYmax)) nY = 0; wX = (nX > 0) ? nX : -nX; wY = (nY > 0) ? nY : -nY; if (wY > wX) wX = wY; Blink_add = (wX > 2) ? wX : 2; switch (stat) { case 0: if ((nX == 0) && (nY == 0)) { if (++homec > 20) stat++; } else homec = 0; sendCar(64, 64, ((ticks1 & (1 << 4)) ? 1 : 0), HN); // Serial.printf("- %3d\n", homec); break; case 1: nX = -nX + 64; nY = -nY + 64; pX = (pX > nX) ? (pX - nX) : (nX - pX); pY = (pY > nY) ? (pY - nY) : (nY - pY); if ((pX < 5) && (pY < 5)) { if (++holdc > 500) powerOFF(); } else holdc = 0; pX = nX; pY = nY; vR = vL = nX; if (nY -= 64) { if (nY > 0) vL -= (nY - HomeYmax); else vR += (nY - HomeYmin); } HL = ((ticks1 & (1 << 2)) ? 1 : 0); sendCar(vL, vR, HL, HN); // Serial.printf("= %3d %3d\n", vL, vR); break; } delay(20); } } |
送信機ソースコード(ADXL345)
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 |
#include <Wire.h> #define I2C_SDA 4 #define I2C_CLK 5 #define ADXL345_ADDR 0x1D void i2c_write_block(uint8_t addr, uint8_t *buff, size_t blen) { Wire.beginTransmission(addr); while (blen-- > 0) { Wire.write(*buff++); delayMicroseconds(30); } Wire.endTransmission(); } void i2c_read_block(uint8_t addr, uint8_t *buff, size_t blen) { Wire.requestFrom(addr, blen); while (blen-- > 0) *buff++ = Wire.read(); } const int BuffB = 10; byte buff[10 + 2 + 6 + (2)] = { 0x2C, 0x0B, // ADXL345_BW_RATE 0x2D, 0x28, // ADXL345_POWER_CTL 0x31, 0x00, // ADXL345_DATA_FORMAT 0x38, 0x00, // ADXL345_FIFO_CTL 0x00, 0x00 // -- }; size_t ADXL345_Init() { Wire.begin(I2C_SDA, I2C_CLK); buff[BuffB] = 0x00; // ADXL345_READ_DEVID i2c_write_block(ADXL345_ADDR, &buff[BuffB], 1); i2c_read_block (ADXL345_ADDR, &buff[BuffB], 1); if (buff[BuffB] != 0xE5) // ADXL345_DEVID_VALUE return(-1); for (int i = 0; (buff[i] != 0x00); i += 2) i2c_write_block(ADXL345_ADDR, &buff[i], 2); return(0); } void ADXL345_readXYZ(int *x, int *y, int *z) { buff[BuffB] = 0x32; // ADXL345_DATAX0; i2c_write_block(ADXL345_ADDR, &buff[BuffB ], 1); i2c_read_block (ADXL345_ADDR, &buff[BuffB + 2], 6); *x = *(int16_t *)&buff[BuffB + 2]; *y = *(int16_t *)&buff[BuffB + 4]; *z = *(int16_t *)&buff[BuffB + 6]; } |
コメント:10
初めまして、何時も楽しく拝見させて頂いてます。
70歳を過ぎた工作好きの爺さんParadiseと申します。
私は以前より、スマホを使わずにESP8266のUDP通信にてROBOTSを制御
したいと考えてましたが、何せプログラムが超初心者で自分でスケッチを書く事が出来ません。
受信回路の方はアンドロイドスマホから上手く制御出来てますが、送信回路の方が上手く働きません。ADXL345は働いてるようですが、DATAを送信できてないようで、原因の特定が出来ていません。
お忙しいと思いますが、時間の有る時で結構ですのでご教示をお願いします。
以下にシリアルモニターのデータの一部を添付しましたので宜しくお願いします。
ADXL345 OK
Connecting to ESPCar
…..
WiFi connected
IP address: 192.168.4.2
Starting UDP
Local port:10000
stat=0
12 0 – 1
11 1 – 2
途中省略
11 0 – 21
stat=1
12 0 = 64 64
13 2 = 64 64
また、この送信機が上手く完成したら、加速度センサーを任天堂Wiiヌンチャクのジョイスティック(I2C信号出力)に替えて操作出来れば使い易いと考えてます。(構想だけですが!)
長くなりましたが、宜しくお願い致します。
2016/04/25 @ 02:30 PM
Paradise より
Paradise様
===
ADXL345 OK
Connecting to ESPCar
…..
WiFi connected
IP address: 192.168.4.2
Starting UDP
Local port:10000
stat=0
12 0 – 1
11 1 – 2
途中省略
11 0 – 21
stat=1
12 0 = 64 64
13 2 = 64 64
===
提示されたログ内容には異常な点はありません。
「stat=0」から「stat=1」までは、コントローラが水平状態になるのを待っているログです。
「stat=1」が表示された以降、コントローラーの傾斜度に応じた左右のモーターの回転指示を送出します。
「12 0 = 64 64」では、傾きが少ないために回転指示は「64=静止」です。
回転指示値は、0 から 127 で 0=最大逆方向回転、64=静止、127=最大正方向回転としています。
「13 2 = 64 64」以降のログをご確認ください。
また、「stat=0」から「stat=1」までの間は、ラジコンカー側のLED(ヘッドライトのつもり(^-^;)が速い点滅をします。
この点滅は、UDPパケットで「ON」と「OFF」を交互に送出しています、この点滅が始まることでラジコンカーとコントローラーのWi-Fi通信が確立されてUDPパケットが送出されていることが確認できます。
解決しない場合は、再度ご連絡ください。
2016/04/27 @ 12:09 AM
goji より
gojiさま、早々のご返事有難うございます。
今朝から再度テストを行いました。
先ず、「13 2 = 64 64」以降は角度に応じた数値の変化を確認出来ていましたが、書くのを怠りお申し訳ありません。
「stat=0」から「stat=1」までの間のラジコンカー側のヘッドライトの点滅がしないので、この辺り(UDP送信)に何か問題があるようです。
スケッチコ-ドのコピーですが、前にADXL345のソースコードを、後にmainのソースコードを配置しましたが、手順に問題ないでしょうか?。
お忙しところを申し訳ありませんが、アドバイスをお願い致します。
2016/04/27 @ 10:43 AM
Paradise より
Paradise様
投稿したソースコードで再実験しましたが、問題は見つけられませんでした。
受信側にデバッグコードを書いて、受信できていないのか、受信した内容が期待している形式と異なっていないかをチェックするしかないかと思います。
「前にADXL345のソースコードを、後にmainのソースコードを配置」
問題ないと思いますが、私は下の画像のようにしています。
(▼ボタンをクリックで「新規タブ」を選択するとタブが増やせます)
2016/04/28 @ 12:33 AM
goji より
gojiさま、お手数をお掛けして申し訳ありません。
今朝から注文してたWroom-02が届いたので送信回路、受信回路共に作り替えてテストしましたが、結果は同じでした。
最初に送信側から受信側に対してATコマンドでのssid, passのやり取りやlocalPortの取得、その後のStarting UDPまではOKのようですが、データーの受信が出来ません。
gojiさまのアドバイスのように受信側にデバッグコードを書いて、受信できていないのか、受信形式等をチェックするしかないようですが、情けないことにソフトウェアが素人なのでデバッグコードが書けず、本日はここで止まってしまいました。
何か良い方法が有ればアドバイスをお願い致します。
ご無理を言って申し訳ありません。 by Paradise
2016/04/28 @ 07:41 PM
Paradise より
Paradise様
「ESP-WROOM-02 Wi-Fiラジコンカー」にある受信側のソースコード「ESP8966CAr.ino」にデバッグコードを書き加えてみてください。
元のソースコード(83行目から)
元のコードを下記のように書き換える(83行目から)
2016/04/30 @ 01:18 AM
goji より
gojiさま、ご親切なアドバイス有難うございます。
このプリント文はアンドロイド版受信コードに入ってたので使ってました。
スマホから受信するとシリアルモニターにDATAが表示されるのですが、送信機からの信号が入りませんでした。
私はESP8266のソース書き込みに自作のNodeMcuを使ってるので、次の方法を試してみました。(ArduinoIDE1.6.8を使用)
マイコンボードの選択箇所のDebug PortをSerialにDebug LevelをWiFiに設定して受信側ソースコードの書き込みを実行すると、シリアルモニターにWiFiの受信状況が表示されました。
その結果、スマホからの受信モニターは次の通りです。
AP IP address:1992.168.4.1
Starting UDP
Local port:10000
wifi evt:7
add 1
aid 1
station: 00:1e:ad:8b:96:ab join, AD = 1
wifi evt:5
wifi evt:7
F=0, C=0,L=110,R=109
とDATAがシリアルモニターに表示されます。
次にESP8266送信機からの受信モニターでは
AP IP address:1992.168.4.1
Starting UDP
Local port:10000
wifi evt:7
err already associad!
station: 18:fe:34:ee:5d:db Leave, AD = 1
rm 1
wifi evt: 6
wifi evt: 7
add 1
aid 1
station: 18:fe:34:ee:5d:db join, AD = 1
wifi evt: 6
wifi evt: 7
とモニターされ、先にLeaveが出るのでjoinしてもタイミングの遅れにより受信できないのではと考えます。
受信機側でjoinを確認後、受信動作をスタートするようにすれば受信できるのではと考えますが、Loopの最初にそのコードを記述する方法が判りません。暫く試行錯誤してみたいと思います。
余談ですが、先に教えて頂いたArduinoIDEのマルチタブはVer1.6.7のバグでScopeエラーが出て使えなかったのですが、3日前にリリースされたVer.1.6.8でバグが解消されて使えるようになりました。
長文になりましたが、初心者の私にお付き合い頂きまして感謝いたします。有難うございました、今後ともよろしくお願い致します。
2016/04/30 @ 11:31 AM
Paradise より
gojiさま、先に送りました文章に一部誤りがありました。
AD = 1をAID = 1に訂正します。
宜しくお願いします。
2016/04/30 @ 02:10 PM
Paradise より
gojiさま、お忙しい処を何度もすみません。
その後色々調べましたが、エラーの原因が判ってきました。
しかし、老人の頭では対処方法が判りません!
受信機側シリアルモニターのエラーコードを記します。
AP IP address: 192.168.4.1
Starting UDP
Local port: 10000
wifi evt: 7
add 1
aid 1
station: 18:fe:34:ee:5b:db join, AID = 1
wifi evt: 5
Fatal exception 2(InstructionRetchErrorCause):
epc1=0x3ffe8ff8, epc2=0x00000000, epc3=0x00000000, excvaddr=0x3ffe8ff8, depc=0x00000000
Exception (2):
epc1=0x3ffe8ff8 epc2=0x00000000 epc3=0x00000000 excvaddr=0x3ffe8ff8 depc=0x00000000
ctx: sys
sp: 3ffffe00 end: 3fffffb0 offset: 01a0
>>>stack>>>
3fffffa0: 3ffe8ff8 40000f49 3fffdab0 40000f49
<<<stack<<<
ets Jan 8 2013,rst cause:2, boot mode:(3,7)
load 0x4010f000, len 1264, room 16
tail 0
chksum 0x0f
csum 0x0f
~ld
ご無理を言って申し訳ありませんが、このコードで原因が特定できるようでしたら、対処方法をお教え下さい。
2016/05/06 @ 10:32 AM
Paradise より
gojiさま、暫くぶりです。
その後、UDP通信のテストを続けてましたが、子機と繋がらない原因がWROOM-02にあることが判りました。
10個買って正常なのが3個しかないのには驚きました。ただ、ATモードでは問題なくUARTモードで問題が起きますが、1個だけファームウエアーを書き換えたらOKなのが有りました。Webで検索しても不良品の情報が無いのが気になります。
私は、ニンテンドウのWiiヌンチャクにWROOM-02を組み込みUDP通信が出来るようになりましたが、この過程で不良品に気付きました。 以上、報告を兼ねてのお便りでしたが、これにて失礼します。有難うございました。
2016/07/13 @ 09:21 PM
Paradise より