WiiBalanceBoardをVRでコントローラにする話、とか

この記事はOculusRiftAdventCalendar2016 11日目の記事です。
qiita.com

はじめに

「確かにルームスケールVRいいんだけど」
「僕の部屋6畳1間で冷蔵庫とかベッドとか食器棚とかあるんで移動できるようなスペースないんだ。」
「なのでWiiのバランスボード的な体重移動式の移動コントローラ、狭い部屋ではいいと思うんだよね」
「そこに乗ってる限りは何かにぶつかる心配とかないし。」
という感じで半年ほど前に作って公開した奴
machinemaker.hatenablog.com
を使ってセ○ウェイ的な移動コントローラのサンプルを作ってみました。
ソース、アプリはこちら。
BalanceBoardTest(Unity).zip - Google ドライブ

※注意:

2バージョンほど制御方式を試しましたが、
PD速度制御（PDControllフォルダのほうの）、直接速度変更（VelocityControllフォルダのほう）
正直どちらもVRで使うとVR酔いして倒れると思うので、各自酔わない使い方を模索していただければと思います。僕が数分で酔って倒れてるのでまだまだ制御方法を考えなくてはいけません…。あくまで使ってみたサンプルとして読んでください。

検証環境：UnityPro4.5.1 (64bit)/Windows10 Home/Oculus Rift CV1

つかいかた：

[共通準備]

・PCとWiiBalanceBoardを接続しておく。
PCとバランスボードのペアリングをします。
通常の方法でペアリングをしても、ボードの電源を切るたびにペアリングが切れてやり直し。
しかしこちらを参考にペアリングをすると再接続もできるようにしてくれます。ありがたや。。。
Wiiリモコン（バランスボード）とWindowsの再接続の話
認識されたバランスボードはHIDデバイスとして認識されています。

[ビルド済みアプリを使ってみる]

0.ディレクトリ名に日本語が含まれないことを確認する
Oculusは英語のことしか考えていないので、
アプリのパスにディレクトリ名にうっかり日本語を使うとVRモードが使えません（！）。
この場合非VRモードで起動します。
（UnityEditor上ではVRで動いてるのにビルドするとVRで動かない、なんで！？ってときはこいつのせいのことが多い。）

なので半角英数にしましょう。

1.Unity製アプリの起動と同時に、別プロセスでTCPサーバアプリをたちあげるので、
「お前怪しいことしてないか？」とアンチウィルスなどの警告が出ることがあります。
（僕の記事を信用するのであれば）ホワイトリストに入れて実行させてあげてください。
（TCPサーバアプリがバランスボードとの通信をしています。
UnityアプリはTCPサーバのクライアントとして、サーバ経由でデータをもらっています。）

※サーバ・クライアント間はTCP/IP通信をしていて、中で
public string DistIPAddress = "127.0.0.1"; //自分のPCを指すアドレス
public int portNum = 8888; //ポート番号
としています。

2.接続がうまくいけば、乗り物や荷重中心を表す球が動くはずです。
体重を左右に移動すると左右に旋回、
体重を前後に移動すると前後に移動となっています。
中身としては前後左右位置に適当なゲインをかけたものを移動/旋回速度目標として設定し、

適当なPD速度制御をかけてやっているだけです。

中身の解説

サーバランチャ

TCPServerLancher.cs:

サーバアプリケーションをフォア/バックグラウンドプロセスとして起動しています。
hideServerApplicationWindowのフラグをTrueにするとバックグラウンド起動します。

using UnityEngine;
using System.Collections;
using System;

// 2016/03/17 written by meka
// 本コード内では
// dobon!!様のDOBON.NET > プログラミング道 > .NET Tips　より、
//	外部プロセスの実行系のコード(MITライセンス）を改変して使用しています。
//(Topページへのリンク　http://dobon.net/)
//http://dobon.net/vb/dotnet/process/openfile.html				（外部プロセスの実行、イベントハンドラの登録）
//http://dobon.net/vb/dotnet/process/processwindowstyle.html	(ウィンドウを非アクティブにして実行)
//http://dobon.net/vb/dotnet/process/shell.html					（StartInfoの使い方）
//http://dobon.net/vb/dotnet/process/killprocesse.html			(外部プロセスを終了させる)
// 以下、dobon!!様のコードのライセンス許諾文表示
/*The MIT License (MIT)
Copyright (c) 2016 DOBON! <http://dobon.net>
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

//Todo::多重起動対策
//Wiimoteサーバーアプリを起動するランチャ

public class WiimoteServerLancher : MonoBehaviour
{
	//外部プロセスのプロセスオブジェクト
	System.Diagnostics.Process wiimoteServerProcess;

	//サーバアプリをバックグラウンドで起動するか
	public bool hideServerApplicationWindow = false;

	//サーバアプリを自動で起動するか サーバアプリの開発時などにfalseにして使う
	public bool lunchTheServerProcess = true;

	//与える引数(現在未使用)
	public string argments = "";

	//このフォルダと.exeはあらかじめ作っておく
	public string applicationPath = "/WiimoteServer/WiimoteTest.exe";

	//クライアントが(Start関数で)起動する前にサーバを起動させたいので、
	//Awakeでサーバを起動する
	void Awake ()
	{
		if(!lunchTheServerProcess)
			return;
		
		string dir = "";
		string filepath = "";				//起動したいアプリケーション
		//エディター時とビルド済み実行ファイルで同じ場所を見るようにする
		//アプリと同じ階層をさがすように設定
		if (Application.isEditor) {
			dir = Application.dataPath + "/..";
		} else {
			dir = Application.dataPath + "/..";
		}
		//起動するファイルのフルパス
		filepath = dir + applicationPath;	

		Debug.Log (filepath);

		//Processオブジェクトを作成する
		wiimoteServerProcess = new System.Diagnostics.Process ();
		//起動するファイルを指定する
		wiimoteServerProcess.StartInfo.FileName = filepath;
		//イベントハンドラの追加
		wiimoteServerProcess.Exited += new EventHandler (wiimoteServerProcess_Exited);
		//プロセスが終了したときに Exited イベントを発生させるように設定
		wiimoteServerProcess.EnableRaisingEvents = true;
		//引数を指定する
		wiimoteServerProcess.StartInfo.Arguments = argments;
		//(フラグを見て)外部プロセスをバックグラウンドで起動するように設定
		if (hideServerApplicationWindow) {
			wiimoteServerProcess.StartInfo.WindowStyle = System.Diagnostics.ProcessWindowStyle.Hidden;
		}
		//起動する
		try {
			wiimoteServerProcess.Start ();
		} catch (Exception e) {
			//外部プロセスをスタートできなかったらエラー表示
			Debug.LogError ("Failed to start process." + e.Message);
		}

	}


	//実行終了時に外部プロセスが生きているなら自動終了させる
	void OnApplicationQuit ()
	{
		if(!lunchTheServerProcess)
			return;
		
		if (wiimoteServerProcess.HasExited) {
			Debug.Log ("外部プロセスは終了済みです");
			return;
		}

		//まだ外部プロセスが動いていれば終了させる。
		try {
			//メインウィンドウを閉じる
			wiimoteServerProcess.CloseMainWindow ();
			//プロセスが終了するまで最大2秒待機する
			wiimoteServerProcess.WaitForExit (2);
			//プロセスが終了したか確認する
			if (wiimoteServerProcess.HasExited) {
				Debug.Log ("外部プロセスが終了しました");
			} else {
				Debug.LogError ("外部プロセスが終了しませんでした。強制終了させます。");
				wiimoteServerProcess.Kill ();		///プロセスを強制終了
			}
		} catch (Exception e) {
			Debug.LogError (e.Message);
		}

	}



	//外部プロセス終了を検知して呼び出される関数
	void wiimoteServerProcess_Exited (object sender, System.EventArgs e)
	{
		if(!lunchTheServerProcess)
			return;
		
		UnityEngine.Debug.Log ("WiimoteProssess_ExitEvent");
		wiimoteServerProcess.Dispose();						//プロセスを破棄
	}




}

通信クライアント系

WiiBalanceBoardCliant.cs

サーバアプリケーションとの間でTCP通信を行い、センサデータを受け取っています。
受け取ったデータはBalanceBoardDatalist.parseMessage(resMsg)で区切って意味のあるデータに変換しています。

using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using System.Threading;
using System.Net;
using System.Net.Sockets;
using System.Text;

// 2016/03/17 written by meka 
// 本コード内では
// dobon!!様のDOBON.NET > プログラミング道 > .NET Tips　より、
// TCP/IP通信系のコード（MITライセンス）を改変して使用しています。
// http://dobon.net/vb/dotnet/internet/tcpclientserver.html 
//(Topページへのリンク　http://dobon.net/)

// 以下ライセンス許諾文表示
/*The MIT License (MIT)
Copyright (c) 2016 DOBON! <http://dobon.net>
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/



[System.Serializable]
public class WiiBalanceBoardCliant : MonoBehaviour
{
	TcpClient tcpClient;
	Thread readThread;
	bool flg_continue = true;
	public string DistIPAddress = "127.0.0.1";					//自分のPCを指すアドレス
	public int portNum = 8888;									//ポート番号
	public int timeout = 2000;

	//通信データ格納用データリスト
	//Wiiバランスボードのインスタンス別に管理
	public BalanceBoardDataList recvBalanceBoardDatalist = new BalanceBoardDataList();

	// Use this for initialization
	void Start ()
	{
		
		IPAddress serverIP = IPAddress.Parse (DistIPAddress);

		if (tcpClient != null)
			return;
		
		tcpClient = new TcpClient ();
		tcpClient.ReceiveTimeout = 2000;	//２秒でタイムアウト
		tcpClient.SendTimeout = 2000;		//２秒でタイムアウト
		tcpClient.Connect (serverIP, portNum);
		Debug.Log ("init client");

		readThread = new Thread (new ThreadStart (recvTask));		//スレッドで呼び出す関数を登録
		readThread.IsBackground = true;
		readThread.Start ();

	}


	void OnDestroy ()
	{
		flg_continue = false;
		//readThreadの終了待ち
		if (readThread != null) {
			Debug.Log ("waiting for read thread kill.");
			readThread.Join ();
			Debug.Log ("read thread killed");
		}
		//TCPクライアント（ソケット）を閉じる
		if (tcpClient != null) {
			Debug.Log ("tcpClient Closing...");
			tcpClient.Close ();
			Debug.Log ("tcpClient Closed.");
			
		}

		
	}

	//受信メッセージを処理
	private void recvTask ()
	{
		//
		NetworkStream ns = tcpClient.GetStream ();
		if(ns == null ){
			Debug.LogError("tcpCliant.Getstream() is failed.");
			return;
		}
		Debug.Log ("GetStream Success");
		//networkStream setup
		ns.ReadTimeout = 2000;
		ns.WriteTimeout = 2000;
		System.Text.Encoding enc = System.Text.Encoding.UTF8;


		byte[] resBytes = new byte[1];									//一文字づつ読ませるため1個ぶん

		//スレッド内で回し続ける
		while (flg_continue) {
			//読み取り、書き込みのタイムアウトを10秒にする
			//デフォルトはInfiniteで、タイムアウトしない
			//(.NET Framework 2.0以上が必要)

			//サーバーから送られたデータを受信する
			System.IO.MemoryStream ms = new System.IO.MemoryStream ();

			int resSize = 0;
			do {
				//データの一部を受信する
				//					Debug.Log ("ns.Read()");
				resSize = ns.Read (resBytes, 0, resBytes.Length);
				//					Debug.Log ("readed... " + resSize + " byte");

				if (!(resSize > 0))
					break;													//通信に返答なし

				//受信したデータをmemoryStreamへ蓄積する
				ms.Write (resBytes, 0, resSize);
				//まだ読み取れるデータがあり、データの最後が\n（改行コード）でない時は受信を続ける
				//改行コードがきたら、それがデータの区切りなので切り出す
			} while(resBytes [resSize - 1] != '\n'&& ns.DataAvailable);

			//受信したデータをmemoryStreamから吐き出す
			string resMsg = enc.GetString (ms.GetBuffer (), 0, (int)ms.Length);
			recvBalanceBoardDatalist.parseMessage(resMsg);
			//holdMessage(resMsg);
			ms.Close ();

		}
		Thread.Sleep (1);
	}


}

BalanceBoardInfo.cs

enum BalanceBoardInfo:通信して流れてきたデータの何番目が何のデータか表す。
class BalanceBoardDataList:複数のバランスボードのデータを保持するリスト.データの解釈・変換もこいつが行う

using UnityEngine;
using System.Collections;
using System.Collections.Generic;

///データ番号定義
///この順番で区切られたメッセージが来る
enum BalanceBoardInfo
{
	index,					//コントローラ番号
	Weight,					//重さ[kg]
	CoPPosX,				//位置[cm]
	CoPPosY,				//位置[cm]
	SensorLoadTopRight,		//センサ値
	SensorLoadTopLeft,		//センサ値
	SenserLoadBottomRight,	//センサ値
	SensorLoadBottomLeft,		//センサ値
	EnumMaxNumber			//個数チェック用
}

//複数のバランスボードのデータを保持するリスト
public class BalanceBoardDataList
{

	public List <BalanceBoardData> balanceBoardData = new List<BalanceBoardData>();
	public string message;	//デバッグ用に保持させる

	//string型のmessageを区切り、数値データとして変換する。
	public bool parseMessage(string message_in)
	{
		//メッセージ異常
		if(message_in == null){
			Debug.LogError("TCP message is null");
			return false;
		}
			
		message = message_in;

		string[] stArryData = message_in.Split(',');
		//Debug.LogWarning(stArryData[0]);
		//Debug.Log("個数："+ stArryData.Length+"要素数:"+(int)WiiBalanceBoardInfo.EnumMaxNumber);
		if(stArryData.Length < (int)BalanceBoardInfo.EnumMaxNumber){
			Debug.LogError("メッセージのデータ個数が不正です。データを破棄します。 個数："+ stArryData.Length+"正しい要素数:"+(int)BalanceBoardInfo.EnumMaxNumber);
			//データ個数が不正
			return false;
		}

		int _index;
		//コントローラ番号を見分ける
		if(!int.TryParse(stArryData[(int)BalanceBoardInfo.index].ToString(),out _index))
			Debug.LogWarning("failed to parse.index:" + stArryData[(int)BalanceBoardInfo.index]);

		//新しいコントローラを追加
		while(balanceBoardData.Count < _index+1 ){
			balanceBoardData.Add(new BalanceBoardData());
			Debug.Log("adding / data.Count:" + balanceBoardData.Count +" index");
		}


		//各値を変換
		//weight[kg]
		if(!float.TryParse(stArryData[(int)BalanceBoardInfo.Weight].ToString(),out balanceBoardData[_index].weight))
			Debug.LogWarning("failed to parse.wight:" + stArryData[(int)BalanceBoardInfo.Weight]);

		//Center of Pressure X[cm]
		if(!float.TryParse(stArryData[(int)BalanceBoardInfo.CoPPosX].ToString(),out balanceBoardData[_index].copPos.x))
			Debug.LogWarning("failed to parse.copX:" + stArryData[(int)BalanceBoardInfo.CoPPosX]);

		//Center of Pressure Y[cm]
		if(!float.TryParse(stArryData[(int)BalanceBoardInfo.CoPPosY].ToString(),out balanceBoardData[_index].copPos.y))
			Debug.LogWarning("failed to parse.copY:" + stArryData[(int)BalanceBoardInfo.CoPPosY]);

		//sensor weight data[]

		if(!float.TryParse(stArryData[(int)BalanceBoardInfo.SensorLoadTopRight].ToString(),out balanceBoardData[_index].sensorLoad.TopRight))
			Debug.LogWarning("failed to parse.SensorKgTopRight:" + stArryData[(int)BalanceBoardInfo.SensorLoadTopRight]);

		if(!float.TryParse(stArryData[(int)BalanceBoardInfo.SensorLoadTopLeft].ToString(),out balanceBoardData[_index].sensorLoad.TopLeft))
			Debug.LogWarning("failed to parse.SensorKgTopLeft:" + stArryData[(int)BalanceBoardInfo.SensorLoadTopLeft]);

		if(!float.TryParse(stArryData[(int)BalanceBoardInfo.SenserLoadBottomRight].ToString(),out balanceBoardData[_index].sensorLoad.BottomRight))
			Debug.LogWarning("failed to parse.SensorKgBottomRight:" + stArryData[(int)BalanceBoardInfo.SenserLoadBottomRight]);

		if(!float.TryParse(stArryData[(int)BalanceBoardInfo.SensorLoadBottomLeft].ToString(),out balanceBoardData[_index].sensorLoad.BottomLeft))
			Debug.LogWarning("failed to parse.SensorKgBottomLeft:" + stArryData[(int)BalanceBoardInfo.SensorLoadBottomLeft]);



		return true;
	}

}




/// <summary>
/// 受信データを意味のある形にしたもの.
/// </summary>
[System.Serializable]
public class BalanceBoardData
{
	/// <summary>
	/// WiiBalanceBoardの各センサにかかっている重量
	/// </summary>
	[System.Serializable]
	public struct SensorLoad
	{
		public float TopRight;
		public float TopLeft;
		public float BottomRight;
		public float BottomLeft;
	}

	public float weight;		//重量[kgf]
	public Vector2 copPos;		//荷重中心[cm]
	public SensorLoad sensorLoad;		//WiiBalanceBoard各センサーにかかっている重量（4か所）
}

表示・移動コントロールなどセンサデータ利用系

WiimoteInfoDisplayBase.cs

WiiBalanceBoardCliantにアクセスしてデータを取得し、加工して表示するための親クラスです。
これの子クラスでOutput関数をオーバーライドして実装する形でセンサデータを利用します。
ボードの複数接続に対応しているので、indexでどのバランスボードの値を使うかを決めています。

using UnityEngine;
using System.Collections;

//情報取得（input）と表示など（Output）の基本クラス
public class WiimoteInfoDisplayBase: MonoBehaviour
{
	///コントローラ番号
	public int index = 0;
	/// <summary>
	//（コントローラの情報をもっている）サーバと通信してるTCPクライアント
	[SerializeField]
	protected WiiBalanceBoardCliant wiiBalanceBoardCliant;
	//バランスボードのステータスを格納する構造体
	[SerializeField]
	protected BalanceBoardData balanceBoardDataNowFlame;		//現在のセンサ情報

	protected BalanceBoardData balanceBoardDataZeroFlame;		//初期センサ情報（オフセット用）


	//[Wii Controller informations]
//	protected BalanceBoardData 

	// Use this for initialization
	protected void Start ()
	{
		//通信クライアントコンポーネントへアクセスできるようにする
//		wiiBalanceBoardCliant = wiiBalanceBoardCliantObject.GetComponent<WiiBalanceBoardCliant> ();
		if (wiiBalanceBoardCliant == null) {
			Debug.LogError ("WiiBalanceBoardCliant is null");
		}
	}

	// Update is called once per frame
	void Update ()
	{
		//WiiバランスボードのInputを取得する
		if (!GetInputData ()) {
			//データが取得できなかったら表示せず次のフレームへ
			return;
		}

		//表示やコントローラとしての利用など。
		//中身は子クラスで実装して下さい
		Output ();
	}

	//
	/// <summary>
	/// Gets the data.
	/// </summary>
	/// <returns><c>true</c>, if data was gotten, <c>false</c> otherwise.</returns>
	bool GetInputData ()
	{
		//通信クライアントにアクセスできない
		if (wiiBalanceBoardCliant == null) {
			Debug.LogError ("BalanceBoardCliant is null");
			return false;
		}
		//バランスボードが接続されていない
		if (wiiBalanceBoardCliant.recvBalanceBoardDatalist.balanceBoardData == null) {
			Debug.LogError ("リストがありません");
			return false;
		}
		//表示対象のコントローラがない
		if (!(index < wiiBalanceBoardCliant.recvBalanceBoardDatalist.balanceBoardData.Count)) {
			Debug.LogWarning ("デバイスの数が足りません index(コントローラ番号[0始まり]):" 
				+ index
				+ " count（総コントローラ数）:"
				+ wiiBalanceBoardCliant.recvBalanceBoardDatalist.balanceBoardData.Count);
			return false;
		}

		//データ取得部
		balanceBoardDataNowFlame = wiiBalanceBoardCliant.recvBalanceBoardDatalist.balanceBoardData [index];
		return true;
	}

	//センサオフセットのゼロ点調整//未実装
	bool calibrateZeroPoint ()
	{
		return true;
	}


	//子クラスで実装する
	protected virtual void Output ()
	{
	}

}

WiiBalanceBoardVisualDisplaySphere.cs

荷重中心の位置を、transform.localPositionに挿入することで球の位置を移動しています。
WiimoteInfoDisplayBaseの子クラスです。

using UnityEngine;
using System.Collections;
/// <summary>
/// 荷重中心の位置を使って軌跡を表示
/// </summary>
public class WiiBalanceBoardVisualDisplaySphere : WiimoteInfoDisplayBase 
{
	protected float gainScale = 0.01f;
	//距離の単位変換ゲイン[cm-> m なので1/100]
	public bool xzInverse = false;			//xz入れ替えて表示

	Vector3 localPos;

	override protected void Output()
	{
		//荷重中心の位置を使ってオブジェクトの位置を上書きする
		if(xzInverse){
			localPos = new Vector3
				(balanceBoardDataNowFlame.copPos.y * gainScale ,
					gameObject.transform.localPosition.y,
					balanceBoardDataNowFlame.copPos.x * gainScale); 
		}else{
			localPos = new Vector3
				( balanceBoardDataNowFlame.copPos.x * gainScale,
					gameObject.transform.localPosition.y,
					-balanceBoardDataNowFlame.copPos.y * gainScale); 

		}

		gameObject.transform.localPosition = localPos;

	


	}
}

VehicleController.cs

セ○ウェイ的コントローラの実装です。物理演算しています。
重心座標に応じて目標速度・目標角速度を決めています。
それに対して速度PD制御することで移動速度・旋回速度を与えています。

using UnityEngine;
using System.Collections;

public class VehicleController : WiimoteInfoDisplayBase {

	///距離の単位変換ゲイン[cm-> m なので1/100]
	protected float gainScale = 0.01f;
	/// <summary>
	/// The speed gain.
	/// </summary>
 
	Rigidbody rigid;

	void Start()
	{
		base.Start();
		if((rigid = GetComponent<Rigidbody>()) == null){
			Debug.LogError("Controller has no rigidbody!");
		}
	}

	/// <summary> </summary>
	public FilteredInput filteredInput = 
		new FilteredInput(
		new Vector3(0.1f,0.1f,0.1f) ,
		new Vector3(0.1f,0.1f,0.1f),
		new Vector3(0.315f , 0.1f, 0.511f) );

	Vector3 velocityTarget;
	Vector3 angVelocityTarget;

	public float forceScale = 10f;
	public float torqueScale = 10f;

	public float PGain;
	public float DGain;
//	public float Igain;
	Vector3 filteredValue;

	/// <summary>
	/// Output this instance.
	/// </summary>
	override protected void Output()
	{
		////入力のフィルタリング
		//荷重中心位置[m]
		Vector3 localPos = new Vector3
			(balanceBoardDataNowFlame.copPos.y * gainScale ,
				gameObject.transform.localPosition.y,
				balanceBoardDataNowFlame.copPos.x * gainScale); 

		//不感帯フィルタ

		filteredValue = filteredInput.GetOutput(localPos);
	}

	Vector3 force = Vector3.zero;
	Vector3 torque = Vector3.zero;

	/// <summary>
	/// 物理演算更新
	/// </summary>
	void FixedUpdate()
	{
		//エラー処理
		if(rigid==null){
			Debug.LogWarning("Controller has no rigidbody!");
			return;
		}

		////乗り物オブジェクトの制御
		/// 
		///////////速度PD制御
		/// 力
		float x;
		float y;
		float z;

		velocityTarget =  Vector3.forward * -filteredValue.x * forceScale;	//目標力の計算

		//トルク
		x = 0f;
		y = filteredValue.z * torqueScale;
		z = 0f;

		angVelocityTarget = new Vector3(x,y,z);
		//並進
		//回転

		//計算座標系はローカルで
		force = (velocityTarget - transform.InverseTransformVector(rigid.velocity)) * PGain + (force)*DGain;
		torque = (angVelocityTarget - transform.InverseTransformDirection(rigid.angularVelocity)) * PGain + (torque)*DGain;


		//addforceはworld座標で
		rigid.AddForce(transform.TransformVector(force), ForceMode.Force);
		rigid.AddTorque(torque,ForceMode.Force);
	}

}

FilteredInput.cs

入力に対してレンジ制限、ゲインの反映、不感帯フィルタ、オフセットを反映するために用意したクラスです。

using UnityEngine;
using System.Collections;


[System.Serializable]
public class FilteredInput
{

	public Vector3 offset = Vector3.zero;
	/// <summary> 不感帯　</summary>
	public Vector3 deadZone = new Vector3(0.05f,0.05f,0.05f);
	/// <summary> 比例ゲイン　</summary>
	public Vector3 pGain = new Vector3(1f,1f,1f);

	//入力範囲制限
	public Vector3 range = new Vector3(0.315f , 0.1f, 0.511f);
	//	Vector3 output;

	public FilteredInput(Vector3 deadZone_in,Vector3 pGain_in,Vector3 range_in)
	{
		deadZone = deadZone_in;
		pGain = pGain_in;
		range = range_in;
	}

	//不感帯フィルタ用
	float DeadFilter(float input_, float deadZone_)
	{
		float output_= 0f;

		if(Mathf.Abs(deadZone_) < Mathf.Abs(input_)){
			if(deadZone_ < input_){
				output_ = input_ - deadZone_;
			}else{
				output_ = input_ + deadZone_;
			}
		}else{
			output_ = 0f;
		}

		return output_;
	}

	//レンジ制限
	float RangeFilter(float input_, float range_)
	{
		float output_ = 0f;
		output_ =  Mathf.Clamp(input_, -range_ * 0.5f , range_ * 0.5f);

		return output_;
	}

	//レンジ制限
	Vector3 RangeFilter(Vector3 input_)
	{
		Vector3 output_ = new Vector3(
			RangeFilter(input_.x,range.x),
			RangeFilter(input_.y,range.y),
			RangeFilter(input_.z,range.z));

		return output_;
	}

	//不感帯フィルタ
	Vector3 DeadFilter(Vector3 input_)
	{
		Vector3 output_ = 
			new Vector3(
				DeadFilter(input_.x,deadZone.x),
				DeadFilter(input_.y,deadZone.y),
				DeadFilter(input_.z,deadZone.z));

		return output_;
	}

	//ゲイン反映
	Vector3 GainFilter(Vector3 input_)
	{
		Vector3 output_ = new Vector3(pGain.x * input_.x,pGain.y * input_.y,pGain.z * input_.z);
		return output_;
	}


	//オフセット設定
	public bool SetOffset(Vector3 offset_in)
	{
		offset = offset_in;
		return true;
	}

	Vector3 OffsetFilter(Vector3 input_)
	{
		Vector3 output_ = input_ - offset;

		return output_;
	}

	//出力取得
	public Vector3 GetOutput(Vector3 input_)
	{

		Vector3 output_ = input_;
		output_ = OffsetFilter(output_);
		output_ = RangeFilter(output_);
		output_ = DeadFilter(output_);
		output_ = GainFilter(output_);
		return 	output_;
	}
}