ソフトウェアコンテスト機体情報
2018年5月25日掲載
以下の機体は大会事務局が作成し、ソフトウェアコンテストで使用する。
外観
ハードウェア構成
主な使用部材と価格
| 部材 | 価格 |
|
東京海洋大学より借用 無料 |
|
|
秋月電子通商 3,990円×2個 |
|
|
3軸 デジタル・コンパス モジュール [HMC5883L使用] （ロボットカーの方位を取得するために搭載） |
アマゾン 300円 |
|
秋月電子通商 2,800円 |
|
|
ドスパラ 10,520円 |
|
|
アマゾン 13,799円 |
|
|
ビックカメラ 2,052円 |
|
GNSS受信機（U-blox NEO-M8T）＆アンテナ
実際の動作の動画
作成中
ソースコード1（情報処理用Arduino）
// 2017/10/7 BluetoothモジュールHC-06は通信が不安定なため、RN42XVP-I/RMに変更
// 2017/10/14 区切り文字に関するバグを修正
// 2017/10/17 RTKLIBから送信されてくるデータの読み取りタイミングを変更
#include <SoftwareSerial.h>
#include <Wire.h>
// 機体制御Arduinoとのシリアル通信
SoftwareSerial SubArduinoSerial(8, 7); // Rx, Tx
// ★Waypointの位置情報
char g_szWP[] = "SLat:35.66737405\r\nSLon:139.79088988\r\n1Lat:35.66743734\r\n1Lon:139.79096864\r\n2Lat:35.66731075\r\n2Lon:139.79081111\r\n---\r\n";
long dSLat = 35667374;
long dSLon = 139790889;
// 位置情報データの送信数
int g_iDataNum = 1;
// 機体制御のコマンドを管理する変数
int g_iRCStatus = 0;
// デジタルコンパス5883
#define HMC5883_WriteAddress 0x1E
#define HMC5883_ModeRegisterAddress 0x02
#define HMC5883_ContinuousModeCommand 0x00
#define HMC5883_DataOutputXMSBAddress 0x03
int regb=0x01;
int regbdata=0x40;
int outputData[6];
// ★デジタルコンパスは中心値がずれるので定期的にオフセットの見直しが必要
// 2017/10/14明治丸前でオフセット実施
int offset_x=116;
int offset_y=307;
void setup()
{
Serial.begin(115200); // RTKLIBからの測位情報受信
Serial1.begin(115200); // Bluetooth通信モジュールでのデータ送信
// SoftwareSerialの初期化
SubArduinoSerial.begin(9600); // 機体制御Arduinoとのシリアル通信用ソフトウェアシリアル
// I2C通信の初期化
Wire.begin(); // デジタルコンパス
// LED PINの指定
pinMode(13, OUTPUT);
}
void loop(void)
{
GetCommand();
char szBuf[512];
int iBuf = ReadRtklibData(szBuf);
if(g_iRCStatus != 0){
// StickPCよりSerialで送信されてくるRTKLIBのデータを受信する
if( iBuf)
{
char szLat[16];
char szLon[16];
// RTKLIBのデータから緯度経度を取り出す
if( !AnalyzeRtklibData( szBuf, szLat, szLon ) )
{
// 緯度、経度を読み取れなかった
return;
}
// デジタルコンパスから車体の向きを取得する（北からの時計回りの向き）
float fDeg;
GetAngle(fDeg);
// float to string
char szDeg[32];
dtostrf(fDeg, 5, 2, szDeg);
Trim(szDeg);
// スタート地点からの距離の計算
char szTempLat[16];
char szTempLon[16];
char szDes[16];
memcpy(szTempLat, szLat, 9);
szTempLat[9] = '\0';
memcpy(szTempLon, szLon, 10);
szTempLon[10] = '\0';
strcpy(&szTempLat[2], &szTempLat[3]);
strcpy(&szTempLon[3], &szTempLon[4]);
long lLat = atol(szTempLat);
long lLon = atol(szTempLon);
lLat = (lLat - dSLat) * 11;
lLon = (lLon - dSLon) * 9;
// 東京海洋大学越中島キャンパス付近では緯度の小数点6桁目が1変化すると0.11ｍ、軽度の小数点6桁目が1変化すると0.09ｍ変化する
double dDes = (lLat*lLat + lLon*lLon );
dDes = sqrt(dDes)/100;
dtostrf(dDes, 5, 1, szDes);
Trim(szDes);
// データをBTで送信する
sprintf(szBuf, "No:%d\r\nLat:%s\r\nLon:%s\r\nDeg:%s\r\nDes:%s\r\n---", g_iDataNum, szLat, szLon, szDeg, szDes);
Serial1.println(szBuf);
g_iDataNum++;
}
}
}
// StickPCよりSerialで送信されてくるRTKLIBのデータを受信する
int ReadRtklibData(char szLineString[])
{
// データ受信したとき
if (Serial.available())
{
int i = 0; // 文字数のカウンタ
while(1)
{
char data[512]; // 文字列格納用
data[i] = Serial.read(); // 1文字ずつ受信
if(data[i] == false)
{
return false;
}
// 終端文字が来たら文字列を引数に収める（or文字数が512以上）
if (i > 510 || data[i] == '\0'|| data[i] == '\r' || data[i] == '\n')
{
data[i] = '\0'; // 末尾に終端文字の挿入
// 一文字目が％だったらインデックスなのでfalseを返す
if(data[0] == '%')
{
return false;
}
strcpy(szLineString, data); // 受信文字列を引数にコピー
return true;
}
else
{
i++;
}
}
}
}
// RTKLIBのデータから緯度経度を取り出す
// 緯度経度が読み取れた場合は、引数変数に、緯度、経度を入れ、戻り値 1 を返す
int AnalyzeRtklibData( char szLineString[], char szfLat[], char szfLon[] )
{
int len;
len = strlen( szLineString );
if(len < 54)
{
return 0;
}
// 緯度
memcpy(szfLat, &szLineString[26], 12);
szfLat[12] = '\0';
if(atof(szfLat) == 0)
{
return 0;
}
memcpy(szfLon, &szLineString[40], 13);
szfLon[13] = '\0';
if(atof(szfLon) == 0)
{
return 0;
}
return 1;
}
// デジタルコンパスから車体の向きを取得する（北からの時計回りの向き）
void GetAngle(float& angle) {
int i,x,y,z;
Wire.beginTransmission(HMC5883_WriteAddress);
Wire.write(regb);
Wire.write(regbdata);
Wire.endTransmission();
Wire.beginTransmission(HMC5883_WriteAddress); //Initiate a transmission with HMC5883 (Write address).
Wire.write(HMC5883_ModeRegisterAddress); //Place the Mode Register Address in send-buffer.
Wire.write(HMC5883_ContinuousModeCommand); //Place the command for Continuous operation Mode in send-buffer.
Wire.endTransmission(); //Send the send-buffer to HMC5883 and end the I2C transmission.
Wire.beginTransmission(HMC5883_WriteAddress); //Initiate a transmission with HMC5883 (Write address).
Wire.requestFrom(HMC5883_WriteAddress,6); //Request 6 bytes of data from the address specified.
//Read the value of magnetic components X,Y and Z
if(6 <= Wire.available()) // If the number of bytes available for reading be <=6.
{
for(i=0;i<6;i++)
{
outputData[i]=Wire.read(); //Store the data in outputData buffer
}
}
x=outputData[0] << 8 | outputData[1]; //Combine MSB and LSB of X Data output register
z=outputData[2] << 8 | outputData[3]; //Combine MSB and LSB of Z Data output register
y=outputData[4] << 8 | outputData[5]; //Combine MSB and LSB of Y Data output register
// 時間が経つと中心値がずれるのでオフセットする
x = x + offset_x;
y = y + offset_y;
// 方位の出力値
angle= atan2((float)y,(float)x) * (180 / 3.14159265) +180;
// ロボットカーの向きと、センサーの取り付け方向の関係から角度を変換
if(angle >= 0 && angle < 270 )
angle = angle + 90;
else
angle = angle - 270;
}
// 走行制御のコマンドを受信する
void GetCommand()
{
if (Serial1.available()){
char val = Serial1.read();
if(val != -1)
{
if(val == 'w'){
Serial1.print(g_szWP);
return;
}
if(g_iRCStatus == 0){
if(val == 'g'){
ready_to_go();
g_iRCStatus = 1;
digitalWrite(13, HIGH);
}
}
else{
switch(val)
{
case 's':
stop();
break;
case 'a':
advance();
break;
case 'p':
prudence();
break;
case 'R':
turn_R();
break;
case 'r':
turn_r();
break;
case 'L':
turn_L();
break;
case 'l':
turn_l();
break;
case 'd':
get_to_destination();
g_iDataNum = 1;
g_iRCStatus = 0;
digitalWrite(13, LOW);
break;
case 'e':
emergency();
g_iDataNum = 1;
g_iRCStatus = 0;
digitalWrite(13, LOW);
break;
default:
stop();
break;
}
}
}
}
}
// スタートの準備
void ready_to_go(void)
{
SubArduinoSerial.print("d");
}
// 止まる
void stop(void)
{
SubArduinoSerial.print("s");
}
// 前進する
void advance(void)
{
SubArduinoSerial.print("a");
}
// ゆっくり前進する
void prudence(void)
{
SubArduinoSerial.print("p");
}
// 鋭く右に曲がる
void turn_R(void)
{
SubArduinoSerial.print("R");
}
// 緩く右に曲がる
void turn_r(void)
{
SubArduinoSerial.print("r");
}
// 鋭く左に曲がる
void turn_L(void)
{
SubArduinoSerial.print("L");
}
// 緩く左に曲がる
void turn_l(void)
{
SubArduinoSerial.print("l");
}
// 目的地に着いた
void get_to_destination(void)
{
SubArduinoSerial.print("d");
}
// 緊急停止
void emergency(void)
{
SubArduinoSerial.print("e");
}
// 文字列の先頭にある空白を削除する
void Trim(char *szLineString)
{
// 先頭から順に空白でない位置を探す
int i = 0;
while ( szLineString[i] != '\0' && szLineString[i] == ' ' )
i++;
strcpy(szLineString, &szLineString[i]);
}
ソースコード２（機体制御用Arduino）
#include <Servo.h>
// 速度
#define add 76 //★80
#define pru 79 //★85
#define stp 100 //★
// 方向
#define Straight 90 //★
#define Turn_R 150 //★150
#define Turn_r 110 //★120
#define Turn_L 30 //★30
#define Turn_l 70 //★60
Servo RCservo;
Servo RCmotor;
void setup()
{
Serial.begin(9600);
Serial1.begin(9600);
// 制御信号を送る出力ピンの設定
RCservo.attach(10); // ラジコンステアリング
RCmotor.attach(11); // ラジコンモーター
// LED
pinMode(3, OUTPUT);
pinMode(4, OUTPUT);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
pinMode(7, OUTPUT);
}
void loop()
{
if (Serial1.available()) {
char inByte;
inByte = Serial1.read();
Serial.println(inByte);
switch(inByte)
{
// 前進
case 'a':
RCservo.write(Straight);
RCmotor.write(add);
digitalWrite(3, LOW);
digitalWrite(4, HIGH);
digitalWrite(5, HIGH);
digitalWrite(6, HIGH);
digitalWrite(7, LOW);
break;
// ゆっくり進む
case 'p':
RCservo.write(Straight);
RCmotor.write(pru);
digitalWrite(3, LOW);
digitalWrite(4, LOW);
digitalWrite(5, HIGH);
digitalWrite(6, LOW);
digitalWrite(7, LOW);
break;
// 鋭く右に曲がる
case 'R':
RCservo.write(Turn_R);
RCmotor.write(add);//pru);
digitalWrite(3, HIGH);
digitalWrite(4, LOW);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(7, LOW);
break;
// 緩く右に曲がる
case 'r':
RCservo.write(Turn_r);
RCmotor.write(add);//pru);
digitalWrite(3, LOW);
digitalWrite(4, HIGH);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(7, LOW);
break;
// 鋭く左に曲がる
case 'L':
RCservo.write(Turn_L);
RCmotor.write(add);//pru);
digitalWrite(3, LOW);
digitalWrite(4, LOW);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(7, HIGH);
break;
// 緩く左に曲がる
case 'l':
RCservo.write(Turn_l);
RCmotor.write(add);//pru);
digitalWrite(3, LOW);
digitalWrite(4, LOW);
digitalWrite(5, LOW);
digitalWrite(6, HIGH);
digitalWrite(7, LOW);
break;
// スタートの準備ができた or 目的地に着いた
case 'd':
RCservo.write(Straight);
RCmotor.write(stp);
digitalWrite(3, HIGH);
digitalWrite(4, LOW);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(7, LOW);
delay(100);
for(int i = 0; i < 3; i++)
{
digitalWrite(3, LOW);
digitalWrite(4, HIGH);
delay(100);
digitalWrite(4, LOW);
digitalWrite(5, HIGH);
delay(100);
digitalWrite(5, LOW);
digitalWrite(6, HIGH);
delay(100);
digitalWrite(6, LOW);
digitalWrite(7, HIGH);
delay(100);
digitalWrite(7, LOW);
digitalWrite(6, HIGH);
delay(100);
digitalWrite(6, LOW);
digitalWrite(5, HIGH);
delay(100);
digitalWrite(5, LOW);
digitalWrite(4, HIGH);
delay(100);
digitalWrite(4, LOW);
digitalWrite(3, HIGH);
delay(100);
}
digitalWrite(3, LOW);
break;
// 緊急停止
case 'e':
RCservo.write(Straight);
RCmotor.write(stp);
for(int i = 0; i < 3; i++)
{
digitalWrite(3, LOW);
digitalWrite(4, LOW);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(7, LOW);
delay(200);
digitalWrite(3, HIGH);
digitalWrite(4, HIGH);
digitalWrite(5, HIGH);
digitalWrite(6, HIGH);
digitalWrite(7, HIGH);
delay(200);
}
break;
// 上記以外⇒停止
default:
RCservo.write(Straight);
RCmotor.write(stp);
digitalWrite(3, HIGH);
digitalWrite(4, LOW);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(7, HIGH);
break;
}
}
}