2015年12月10日木曜日

IchigoJam で遊ぶ その6 IchigoStorms

　IchitoJam で遊ぶ 第6弾は LEGO Mindstorms を IchigoJam で制御してみようと思います。ただし、IchigoJam でダイレクトに LEGO Mindstorms のセンサとシリアル通信したり、サーボモータのエンコーダパルスをカウントしたりするのは難しいので（モータのPWM駆動くらいはIchigoJam単体でイケルと思いますが）、センサとの通信とモータエンコーダのカウント及びモータPWMは ATMega 328P （に Arduinoのファームウェアを書き込んだもの）に行わせて、IchigoJam は ATMega と シリアル通信することで、制御を行います。

　IchigoJam にスタックできるシールド形状の基板を秋月電子のパワーグリッドユニバーサル基板で作成しました。外部からの 6～9V前後の給電をレギュレータで5Vにしてシールド側の電源とするとともに、IchigoJam側のUSBコネクタに繋がるソケットに供給しています。
　実はこの基板の製作で一番苦労したのは、LEGO Mindstorms で使われている特殊なモジュラーコネクタを用意することでした。一見、電話（ISDN）などに使われる6極のモジュラーに見えますが、爪の位置が真ん中ではなくコネクタの穴に向かって覗きこむように見ると右側にずれています。この特殊なコネクタは Robo Product さんで入手しました。



　この IchigoJam + シールド を LEGO と結合できるようにするために、以前に BrickPi のケースを新型のものに取り替えた時に余った旧型の BrickPi のRaspberry Pi を固定するためのアクリルケース（ボード）を流用することにしました。
　IchigoJam と Raspberry Pi の穴位置を変換するスペーサマウントをアクリル板で自作して、取り付けました。





　IchigoStorms シールドを載せて、


　IchigoStorms 本体は、こんな感じで出来上がりました。



　超音波センサとサーボモータ大を2つ使って、簡単なタンクのモデルを組んでみました。





　IchigoJam の BASIC で簡単な障害物回避プログラム（超音波センサがある距離内で検知したら、左に曲がる）を試しに動かしてみました。


　ソースは・・・、そういてば、本体に載せたIchigoJamで直接プログラミングして、パソコンにダウンロードしてないや。あとで、追加で掲載する予定です。

　以下、落書きレベルですが、回路図のpng画像です。


以下が、ATMega328P側のコード（Arduinoスケッチ）になります。 EV3UARTSensor というライブラリを利用させてもらっています。このライブラリはこちらから頂いたものです。
#include <SoftwareSerial.h>
#include <EV3UARTSensor.h>
// -- Grobal Variable --
const int MAT0 = 2; // MAT0 : Motor A's tacho0 pin.
const int MAT1 = 4; // MAT1 : Motor A's tacho1 pin.
const int MAO0 = 6; // MAO0 : Motor A's output0(PWM) pin.
const int MAO1 = 7; // MAO1 : Motor A's output1(DIR) pin.
const int MBT0 = 3; // MBT0 : Motor B's tacho0 pin.
const int MBT1 = 5; // MBT1 : Motor B's tacho1 pin.
const int MBO0 = 9; // MBO0 : Motor B's output0(PWM) pin.
const int MBO1 = 8; // MBO1 : Motor B's output1(DIR) pin.
int MA_speed = 0; // Motor A speed.
int MB_speed = 0; // Motro B speed.
long MA_enc = 0; // Motor A encorder count.
long MB_enc = 0; // Motor B encorder count.
const int CW = 0; // Motor rotary direction CW.
const int CCW = 1; // Motor rotary direction CCW.
int MA_dir = 0; // Motor A rotary direction.
int MB_dir = 0; // Motor B rotary direction.
unsigned long lastMessage = 0;
EV3UARTSensor S1(10, 11);
//EV3UARTSensor S2(12, 13);
void setup()
{
// Initialize serial port.
Serial.begin(115200);
// Sensor Initialization.
S1.begin();
//S2.begin();
// Motor A pin configuration.
pinMode(MAT0, INPUT);
attachInterrupt(0, MAT0_changed, CHANGE);
pinMode(MAT1, INPUT);
pinMode(MAO0, OUTPUT);
pinMode(MAO1, OUTPUT);
// Motor B pin configuration.
pinMode(MBT0, INPUT);
attachInterrupt(1, MBT0_changed, CHANGE);
pinMode(MBT1, INPUT);
pinMode(MBO0, OUTPUT);
pinMode(MBO1, OUTPUT);
}
void loop()
{
char buf[20];
// Update sensor.
S1.check_for_data();
//S2.check_for_data();
float sample1[S1.sample_size()];
S1.fetch_sample(sample1, 0);
//float sample2[S2.sample_size()];
//S2.fetch_sample(sample2, 0);
if (millis() - lastMessage > 100)
{
if (Serial.available() > 0)
{
char cmd = Serial.read();
switch (cmd)
{
case '#':
break;
case 'D':
// D + A or B + ?(Query Drive Speed) or Value(Set Drive Speed).
while (Serial.available() == 0);
cmd = Serial.read();
switch (cmd)
{
case 'A':
while (Serial.available() == 0);
cmd = Serial.read();
switch (cmd)
{
case '?':
Serial.println(MA_speed);
break;
default:
int i;
for (i = 0; i < 20; i++)
buf[i] = 0;
buf[0] = cmd;
for (i = 1; i < 20; i++)
{
while (Serial.available() == 0);
buf[i] = Serial.read();
if ((buf[i] == 0x0d) || (buf[i] == '#'))
break;
}
buf[i] = 0;
MA_speed = atoi(buf);
MA_speed = set_motor_speed_A(MA_speed);
break;
}
break;
case 'B':
while (Serial.available() == 0);
cmd = Serial.read();
switch (cmd)
{
case '?':
Serial.println(MB_speed);
break;
default:
int i;
for (i = 0; i < 20; i++)
buf[i] = 0;
buf[0] = cmd;
for (i = 1; i < 20; i++)
{
while (Serial.available() == 0);
buf[i] = Serial.read();
if ((buf[i] == 0x0d) || (buf[i] == '#'))
break;
}
buf[i] = 0;
MB_speed = atoi(buf);
MB_speed = set_motor_speed_B(MB_speed);
break;
}
break;
default:
break;
}
break;
case 'E':
// E + A or B + ?(Query Encoder Count) or R(Reset Encoder)
while (Serial.available() == 0);
cmd = Serial.read();
switch (cmd)
{
case 'A':
while (Serial.available() == 0);
cmd = Serial.read();
switch (cmd)
{
case '?':
Serial.println(MA_enc);
break;
case 'R':
MA_enc = 0;
break;
default:
break;
}
break;
case 'B':
while (Serial.available() == 0);
cmd = Serial.read();
switch (cmd)
{
case '?':
Serial.println(MB_enc);
break;
case 'R':
MB_enc = 0;
break;
default:
break;
}
break;
default:
break;
}
break;
case 'S':
// S + 1 or 2 + ?(Query Sensor Value).
while (Serial.available() == 0);
cmd = Serial.read();
switch (cmd)
{
case '1':
while (Serial.available() == 0);
cmd = Serial.read();
switch (cmd)
{
case '?':
for (int i = 0; i < S1.sample_size(); i++)
{
Serial.print((int)sample1[i]);
Serial.print(" ");
}
Serial.println();
break;
default:
break;
}
case '2':
while (Serial.available() == 0);
cmd = Serial.read();
switch (cmd)
{
case '?':
/*
for (int i = 0; i < S2.sample_size(); i++)
{
Serial.print((int)sample2[i]);
Serial.print(" ");
}
Serial.println();
*/
break;
default:
break;
}
default:
break;
}
break;
default:
break;
}
}
lastMessage = millis();
}
}
// Motor A encorder operation.
void inc_enc_MA()
{
if (MA_enc >= 999999999)
MA_enc = -999999999;
else
++MA_enc;
}
void dec_enc_MA()
{
if (MA_enc <= -999999999)
MA_enc = 999999999;
else
--MA_enc;
}
int get_enc_MA()
{
return MA_enc;
}
void clear_enc_MA()
{
MA_enc = 0;
}
// Motor B encorder operation.
void inc_enc_MB()
{
if (MB_enc >= 999999999)
MB_enc = -999999999;
else
++MB_enc;
}
void dec_enc_MB()
{
if (MB_enc <= -999999999)
MB_enc = 999999999;
else
--MB_enc;
}
int get_enc_MB()
{
return MB_enc;
}
void clear_enc_MB()
{
MB_enc = 0;
}
// MAT0 pin status changed event handler.
void MAT0_changed()
{
if (digitalRead(MAT0))
{
if (digitalRead(MAT1))
dec_enc_MA();
else
inc_enc_MA();
}
else
{
if (digitalRead(MAT1))
inc_enc_MA();
else
dec_enc_MA();
}
}
// MBT0 pin status changed event handler.
void MBT0_changed()
{
if (digitalRead(MBT0))
{
if (digitalRead(MBT1))
dec_enc_MB();
else
inc_enc_MB();
}
else
{
if (digitalRead(MBT1))
inc_enc_MB();
else
dec_enc_MB();
}
}
// Motor A set motor speed.
int set_motor_speed_A(int speed)
{
if (speed >= 0)
{
analogWrite(MAO0, 0);
digitalWrite(MAO1, LOW);
if (speed > 255) speed = 255;
if (speed < 0) speed = 0;
analogWrite(MAO0, speed);
return speed;
}
else
{
analogWrite(MAO0, 0);
digitalWrite(MAO1, HIGH);
speed = 255 + speed;
if (speed > 255) speed = 255;
if (speed < 0) speed = 0;
analogWrite(MAO0, speed);
return speed - 255;
}
}
// Motor B set motor speed.
int set_motor_speed_B(int speed)
{
if (speed >= 0)
{
analogWrite(MBO0, 0);
digitalWrite(MBO1, LOW);
if (speed > 255) speed = 255;
if (speed < 0) speed = 0;
analogWrite(MBO0, speed);
return speed;
}
else
{
analogWrite(MBO0, 0);
digitalWrite(MBO1, HIGH);
speed = 255 + speed;
if (speed > 255) speed = 255;
if (speed < 0) speed = 0;
analogWrite(MBO0, speed);
return speed - 255;
}
}
view raw IchigoStorms.ino hosted with ❤ by GitHub

IchigoJam の BASICコードは以下のとおり。
10 ?"#DA100#"
20 ?"#DB100#"
30 ?"#S1?#"
40 INPUT A
50 IF A>200 THEN GOTO 10
60 ?"#DA-100#"
70 GOTO 30
view raw IchigoStorms.bas hosted with ❤ by GitHub
投稿者 時刻: 0 件のコメント:
ラベル: , , , , , , , ,
登録: 投稿 (Atom)