FASTRAKをC#で使う

実際にFASTRAKを使ったプログラミングを行っていきます。

ここではフォームにファストラックのデータを表示させるプログラムを作成します。

準備

 　VisualC#で"fastrak_test"というWindowsフォームアプリケーションのプロジェクトを作成します。

フォームの作成

　フォームにボタンを3つ、チェックボックスを1つ、ラベルを6つ貼り付けます。

　ボタンのテキストを"接続"、"切断"、"リクエスト"に変更します。チェックボックスのテキストを"自動"にします。

　各ボタン、チェックボックスをダブルクリックしてイベントを登録します。

デモプログラム

　Forn1.csに以下のプログラムを貼り付けます。"COM3"の部分は各自の環境に

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.IO.Ports;

namespace fastrak_test
{
    public partial class Form1 : Form
    {
        private SerialPort serialPort;//コンポーネント

        public Form1()
        {
            InitializeComponent();

            //他スレッドからのコントロール呼び出し許可
            Control.CheckForIllegalCrossThreadCalls = false;  

            //シリアルポート
            this.serialPort = new System.IO.Ports.SerialPort();
            this.serialPort.DataReceived += 
                new System.IO.Ports.SerialDataReceivedEventHandler(this.serialPort_DataReceived);

            this.serialPort.PortName = "COM3";//ポート番号（＊自分の環境に合わせて変更すること）
            this.serialPort.BaudRate = 115200;//ビットレート
            this.serialPort.DataBits = 8;
            this.serialPort.Parity = Parity.None;
            this.serialPort.Handshake = Handshake.XOnXOff;
            this.serialPort.Parity = Parity.None;
            this.serialPort.StopBits = StopBits.One;
        }

        //接続
        private void button1_Click(object sender, EventArgs e)
        {
            try
            {
                this.serialPort.Open();

                //設定
                this.serialPort.WriteLine("u"); //インチからセンチへ
            }
            catch
            {
                System.Windows.Forms.MessageBox.Show("ポートを開けませんでした");
            }
        }

        //切断
        private void button2_Click(object sender, EventArgs e)
        {
            this.serialPort.Close();
        }

        //リクエスト
        private void button3_Click(object sender, EventArgs e)
        {
            if (this.serialPort.IsOpen == true)
            {
                this.serialPort.WriteLine("P");
            }
        }
        
        //チェックボックス
        private void checkBox1_CheckedChanged(object sender, EventArgs e)
        {
            if (this.checkBox1.Checked == true)
            {
                this.serialPort.Write("C");
            }
            else
            {
                this.serialPort.Write("c");
            }
        }

        //データ受信
        private void serialPort_DataReceived(object sender,
            System.IO.Ports.SerialDataReceivedEventArgs e)
        {
            int no = 0;//レシーバ番号

            double TX = 0;
            double TY = 0;
            double TZ = 0;

            double RX = 0;
            double RY = 0;
            double RZ = 0;

            try
            {
                SerialPort port = (SerialPort)sender;
                string s = port.ReadLine();

                //レシーバー番号を取得
                no = Convert.ToInt32(s.Substring(0, 2)) - 1;

                //位置を取得
                TX = Convert.ToDouble(s.Substring(3, 7));
                TY = Convert.ToDouble(s.Substring(10, 7));
                TZ = Convert.ToDouble(s.Substring(17, 7));

                RX = Convert.ToDouble(s.Substring(24, 7));
                RY = Convert.ToDouble(s.Substring(31, 7));
                RZ = Convert.ToDouble(s.Substring(38, 7));

                if (no == 0)
                {
                    this.label1.Text = "TX:" + TX.ToString();
                    this.label2.Text = "TY:" + TY.ToString();
                    this.label3.Text = "TZ:" + TZ.ToString();

                    this.label4.Text = "RX:" + RX.ToString();
                    this.label5.Text = "RY:" + RY.ToString();
                    this.label6.Text = "RZ:" + RZ.ToString();
                }
            }
            catch
            {
                Console.WriteLine("データ受信エラー");
            }
        }
    }
}

実行

1.Fastrakの電源を入れて、緑のランプの点滅が終わるのを待ちます。

2.F5キーを押して実行してください。

3.接続ボタンを押し、接続します。このままでは何もおこりません。

4.リクエストボタンを1度押します。ラベルに数字が表示されます。

この値はレシーバ1のデータを表しています。レシーバ1を動かして、リクエストボタンを押してみてください。値が変わります。

5.毎回リクエストボタンを押すのは大変なので、"自動"と書かれたチェックボックスをチェックさせます。値がリアルタイムに表示されます。

6.切断ボタンを押して終了します。

解説

            this.serialPort.PortName = "COM3";
            this.serialPort.BaudRate = 115200;//ビットレート
            this.serialPort.DataBits = 8;
            this.serialPort.Parity = Parity.None;
            this.serialPort.Handshake = Handshake.XOnXOff;
            this.serialPort.Parity = Parity.None;
            this.serialPort.StopBits = StopBits.One;

フォームの コンストラクタでシリアル通信の設定を行っています。

                this.serialPort.Open();
                this.serialPort.WriteLine("u");

接続ボタンを押すとポートを開いています。直後にuコマンドを送信しています。uコマンドはFastakのデータをインチからセンチに変更するコマンドです。

　　　　　　　this.serialPort.WriteLine("P");

リクエストボタンはPコマンドを送信しています。Pコマンドを送るとFASTRAKは1回だけデータを送ってきます。 

　　　　　　　this.serialPort.Write("C");

自動チェックボックスではCコマンドを送信しています。Cコマンドを送るとc(小文字)コマンドが送られてくるまでデータを送信し続けます。

　　　　　　　string s = port.ReadLine();
                no = Convert.ToInt32(s.Substring(0, 2)) - 1;
                TX = Convert.ToDouble(s.Substring(3, 7));
                TY = Convert.ToDouble(s.Substring(10, 7));
                TZ = Convert.ToDouble(s.Substring(17, 7));

                RX = Convert.ToDouble(s.Substring(24, 7));
                RY = Convert.ToDouble(s.Substring(31, 7));
                RZ = Convert.ToDouble(s.Substring(38, 7));

データ受信部分では、送られてきたデータを解析し、ラベルに表示させています。

特に設定を変更していない時は、FASTRAKから送られてくるデータはアスキーでこのような構成になっています。

01   -7.60 -30.70   4.31-177.65  -1.28   6.34
02   22.84  -7.06   8.86  -6.52  -3.96 -48.27

 レシーバ番号(2バイト),スペース(1バイト),X座標(7バイト),Y座標(7バイト),Z座標(7バイト),ピッチ(7バイト),ヨー(7バイト),ロール(7バイト),改行(1バイト)

それぞれの値の位置が固定されているので、String.Substringで値を取り出しConvert.To○○で数字に変換しています。

noには0から始まるレシーバ番号が記録されているので、最初のレシーバの時のみラベルの値を変更しています。