Android_Udp.rb

SimpleWiFi.java

import java.io.UnsupportedEncodingException;


import android.app.Activity;
import android.graphics.Color;
import android.os.Bundle;
import android.os.Handler;
import android.view.View;
import android.view.Window;
import android.view.View.OnClickListener;
import android.widget.Button;
import android.widget.LinearLayout;
import android.widget.TextView;
import android.widget.RelativeLayout;
//
//	SimpleWiFi main Activity
//
public class SimpleWiFi extends Activity
{
	//	Common variables
	String ModuleAdrs = "192.168.1.5";    // WiFi module IP address (edit it please)

	Udp UdpObj;    // UDP object
	Thread DispThread;    // Display thread object
	volatile boolean abortflag;    // Thread abort flag
	Handler hHandler;    // Drawing handler
	int	Condition;	//
	// バッファ、一般変数定義
	private String[] gyairo;
	private String temp;
	private boolean State = false;
	private final Handler handler = new Handler();
	private final Handler _handler = new Handler();
	// クラス変  Handler _handler = new Handler();数、定数の宣言
	private final static int WC = LinearLayout.LayoutParams.WRAP_CONTENT;
	private static Button  button2, button3, button4, button5, button6, button7, button8;
	private static TextView text1, text2, text3, text4, text5, text6, text7, text8 ;
	private boolean flag = false;

	// 制御コマンド定数定義
	private static final char Forward = 0x66; //f
	private static final char Back =  0x62; //b
	private static final char L_turn = 0x6C; //l
	private static final char R_turn = 0x72; //r
	private static final char Stp = 0x73; //s
	private static final char Measure = 0x6D; //m
	private static final char Shut =0x65; //e

	// private GoogleApiClient client;
	//	onCreate
	/*******
	 * 最初に実行する関数
	 ********/
	@Override
	public void onCreate(Bundle savedInstanceState) {
		super.onCreate(savedInstanceState);
		hHandler = new Handler();    // Create UI handler
		abortflag = false;    // Clear abort flag first
		requestWindowFeature(Window.FEATURE_NO_TITLE);
		/*** レイアウト定義　*******/
		RelativeLayout relativeLayout = new RelativeLayout(this);
		setContentView(relativeLayout);
		//モータ制御表示
		text1 = new TextView(this);
		text1.setId(1);
		text1.setTextSize(20f);
		text1.setTextColor(Color.MAGENTA);
		text1.setText("モータ制御");
		relativeLayout.addView(text1, createParam(WC, WC));
		//前進表示
		button2 = new Button(this);
		button2.setId(2);
		button2.setBackgroundColor(Color.YELLOW);
		button2.setTextSize(18f);
		button2.setTextColor(Color.BLACK);
		button2.setText("前進");
		RelativeLayout.LayoutParams param2 = createParam(WC, WC);
		param2.addRule(RelativeLayout.RIGHT_OF, 1);
		relativeLayout.addView(button2, param2);
		param2.setMargins(15, 15, 15, 15);
		//後退表示
		button3 = new Button(this);
		button3.setId(3);
		button3.setBackgroundColor(Color.YELLOW);
		button3.setTextSize(18f);
		button3.setTextColor(Color.BLACK);
		button3.setText("後退");
		RelativeLayout.LayoutParams param3 = createParam(WC, WC);
		param3.addRule(RelativeLayout.BELOW, 2);
		param3.addRule(RelativeLayout.ALIGN_RIGHT, 2);
		relativeLayout.addView(button3, param3);
		param3.setMargins(15, 15, 15, 15);
		//左回転表示
		button4 = new Button(this);
		button4.setId(4);
		button4.setBackgroundColor(Color.YELLOW);
		button4.setTextSize(18f);
		button4.setTextColor(Color.BLACK);
		button4.setText("左回転");
		RelativeLayout.LayoutParams param4 = createParam(WC, WC);
		param4.addRule(RelativeLayout.BELOW, 2);
		param4.addRule(RelativeLayout.LEFT_OF, 2);
		relativeLayout.addView(button4, param4);
		param4.setMargins(15, 15, 40, 15);
		//右回転表示
		button5 = new Button(this);
		button5.setId(5);
		button5.setBackgroundColor(Color.YELLOW);
		button5.setTextSize(18f);
		button5.setTextColor(Color.BLACK);
		button5.setText("右回転");
		RelativeLayout.LayoutParams param5 = createParam(WC, WC);
		param5.addRule(RelativeLayout.BELOW, 2);
		param5.addRule(RelativeLayout.RIGHT_OF, 2);
		relativeLayout.addView(button5, param5);
		param5.setMargins(15, 15, 15, 15);
		//STOP　表示
		button6 = new Button(this);
		button6.setId(6);
		button6.setBackgroundColor(Color.YELLOW);
		button6.setTextSize(18f);
		button6.setTextColor(Color.BLACK);
		button6.setText("STOP");
		RelativeLayout.LayoutParams param6 = createParam(WC, WC);
		param6.addRule(RelativeLayout.BELOW, 3);
		param6.addRule(RelativeLayout.RIGHT_OF, 3);
		param6.addRule(RelativeLayout.ALIGN_RIGHT, 5);
		relativeLayout.addView(button6, param6);
		param6.setMargins(35, 15, 15, 100);
		//ジャイロセンサ表示
		text2 = new TextView(this);
		text2.setId(7);
		text2.setTextSize(18f);
		text2.setTextColor(Color.MAGENTA);
		text2.setText("ジャイロセンサ");
		RelativeLayout.LayoutParams param7 = createParam(WC, WC);
		param7.addRule(RelativeLayout.BELOW, 6);
		param7.addRule(RelativeLayout.LEFT_OF, 6);
		param7.addRule(RelativeLayout.ALIGN_LEFT, 4);
		relativeLayout.addView(text2, param7);
		param7.setMargins(15, 15, 15, 15);
		//Measure 表示
		button7 = new Button(this);
		button7.setId(8);
		button7.setBackgroundColor(Color.YELLOW);
		button7.setTextSize(18f);
		button7.setTextColor(Color.BLACK);
		button7.setText("計測");
		RelativeLayout.LayoutParams param8 = createParam(WC, WC);
		param8.addRule(RelativeLayout.BELOW, 6);
		param8.addRule(RelativeLayout.RIGHT_OF, 6);
		param8.addRule(RelativeLayout.ALIGN_LEFT, 6);
		relativeLayout.addView(button7, param8);
		param8.setMargins(15, 15, 15, 50);
		//X　表示
		text3 = new TextView(this);
		text3.setId(9);
		text3.setTextSize(20f);
		text3.setTextColor(Color.WHITE);
		text3.setText("X");
		RelativeLayout.LayoutParams param9 = createParam(WC, WC);
		param9.addRule(RelativeLayout.BELOW, 8);
		param9.addRule(RelativeLayout.LEFT_OF, 8);
		param9.addRule(RelativeLayout.ALIGN_RIGHT, 7);
		relativeLayout.addView(text3, param9);
		param9.setMargins(15, 15, 15, 50);
		//X value 表示
		text4 = new TextView(this);
		text4.setId(10);
		text4.setTextSize(20f);
		text4.setTextColor(Color.WHITE);
		text4.setText("0.0");
		RelativeLayout.LayoutParams param10 = createParam(WC, WC);
		param10.addRule(RelativeLayout.BELOW, 8);
		param10.addRule(RelativeLayout.RIGHT_OF, 8);
		param10.addRule(RelativeLayout.ALIGN_LEFT, 6);
		relativeLayout.addView(text4, param10);
		param10. setMargins(15, 15, 15, 50);
		//Y　表示
		text5 = new TextView(this);
		text5.setId(11);
		text5.setTextSize(20f);
		text5.setTextColor(Color.WHITE);
		text5.setText("Y");
		RelativeLayout.LayoutParams param11 = createParam(WC, WC);
		param11.addRule(RelativeLayout.BELOW, 9);
		param11.addRule(RelativeLayout.ALIGN_LEFT, 9);
		relativeLayout.addView(text5, param11);
		param11.setMargins(15, 15, 15, 50);
		//Y value 表示
		text6 = new TextView(this);
		text6.setId(12);
		text6.setTextSize(20f);
		text6.setTextColor(Color.WHITE);
		text6.setText("0.0");
		RelativeLayout.LayoutParams param12 = createParam(WC, WC);
		param12.addRule(RelativeLayout.BELOW, 10);
		param12.addRule(RelativeLayout.ALIGN_LEFT, 6);
		relativeLayout.addView(text6, param12);
		param12.setMargins(15, 15, 15, 50);
		//Z　表示
		text7 = new TextView(this);
		text7.setId(13);
		text7.setTextSize(20f);
		text7.setTextColor(Color.WHITE);
		text7.setText("Z");
		RelativeLayout.LayoutParams param13 = createParam(WC, WC);
		param13.addRule(RelativeLayout.BELOW, 11);
		param13.addRule(RelativeLayout.ALIGN_LEFT, 11);
		relativeLayout.addView(text7, param13);
		param13.setMargins(15, 15, 15, 50);
		//Z value 表示
		text8 = new TextView(this);
		text8.setId(14);
		text8.setTextSize(20f);
		text8.setTextColor(Color.WHITE);
		text8.setText("0.0");
		RelativeLayout.LayoutParams param14 = createParam(WC, WC);
		param14.addRule(RelativeLayout.BELOW, 12);
		param14.addRule(RelativeLayout.ALIGN_LEFT, 6);
		relativeLayout.addView(text8, param14);
		param14.setMargins(15, 15, 15, 60);
		//End 表示
		button8 = new Button(this);
		button8.setId(15);
		button8.setBackgroundColor(Color.YELLOW);
		button8.setTextSize(18f);
		button8.setTextColor(Color.BLACK);
		button8.setText("END");
		RelativeLayout.LayoutParams param15 = createParam(WC, WC);
		param15.addRule(RelativeLayout.BELOW, 14);
		param15.addRule(RelativeLayout.RIGHT_OF, 14);
		param15.addRule(RelativeLayout.ALIGN_LEFT, 14);
		relativeLayout.addView(button8, param15);
		param8.setMargins(15, 15, 15, 15);
		// ボタンイベントリスナ生成
		button2.setOnClickListener(new OnClickListener() {
			public void onClick(View v) {
				String  c = String.valueOf(Forward);
				byte [] command = c.getBytes();
				// コマンド送信
				UdpObj.Send(command, command.length);
				button2.setBackgroundColor(Color.RED);
				button3.setBackgroundColor(Color.YELLOW);
				button4.setBackgroundColor(Color.YELLOW);
				button5.setBackgroundColor(Color.YELLOW);
				button6.setBackgroundColor(Color.YELLOW);
			}
		});
		button3.setOnClickListener(new OnClickListener() {
			public void onClick(View v) {
				String  c = String.valueOf(Back);
				byte [] command = c.getBytes();
				// コマンド送信
				UdpObj.Send(command, command.length);
				button3.setBackgroundColor(Color.RED);
				button2.setBackgroundColor(Color.YELLOW);
				button4.setBackgroundColor(Color.YELLOW);
				button5.setBackgroundColor(Color.YELLOW);
				button6.setBackgroundColor(Color.YELLOW);
			}
		});
		button4.setOnClickListener(new OnClickListener() {
			public void onClick(View v) {
				String  c = String.valueOf(L_turn);
				byte [] command = c.getBytes();
				// コマンド送信
				UdpObj.Send(command, command.length);
				button4.setBackgroundColor(Color.RED);
				button3.setBackgroundColor(Color.YELLOW);
				button2.setBackgroundColor(Color.YELLOW);
				button5.setBackgroundColor(Color.YELLOW);
				button6.setBackgroundColor(Color.YELLOW);
			}
		});
		button5.setOnClickListener(new OnClickListener() {
			public void onClick(View v) {
				String  c = String.valueOf(R_turn);
				byte [] command = c.getBytes();
				// コマンド送信
				UdpObj.Send(command, command.length);
				button5.setBackgroundColor(Color.RED);
				button3.setBackgroundColor(Color.YELLOW);
				button4.setBackgroundColor(Color.YELLOW);
				button2.setBackgroundColor(Color.YELLOW);
				button6.setBackgroundColor(Color.YELLOW);
			}
		});
		button6.setOnClickListener(new OnClickListener() {
			public void onClick(View v) {
				String  c = String.valueOf(Stp);
				byte [] command = c.getBytes();
				// コマンド送信
				UdpObj.Send(command, command.length);
				button6.setBackgroundColor(Color.RED);
				button3.setBackgroundColor(Color.YELLOW);
				button4.setBackgroundColor(Color.YELLOW);
				button5.setBackgroundColor(Color.YELLOW);
				button2.setBackgroundColor(Color.YELLOW);
			}
		});
		button7.setOnClickListener(new OnClickListener() {
			public void onClick(View v) {
			if(State == false) {
				String  c = String.valueOf(Measure);
				byte [] command = c.getBytes();
				// コマンド送信
				UdpObj.Send(command, command.length);
				button7.setText("停止");
				_handler.postDelayed(new Runnable() {//1秒毎に計測コマンド送信
					public void run() {
					String  c = String.valueOf(Measure);
					byte [] command = c.getBytes();
					// コマンド送信
					UdpObj.Send(command, command.length);
					handler.postDelayed(this, 1000);
					}
				}, 1000);
				flag = true;
			}else {
				button7.setText("計測");
				_handler.removeCallbacksAndMessages(null);//計測コマンド送信停止
				flag = false;
			}
				State = !State;
			}
		});
		button8.setOnClickListener(new OnClickListener() {
			public void onClick(View v) {
				String  c = String.valueOf(Shut);
				byte [] command = c.getBytes();;
				// コマンド送信
				UdpObj.Send(command, command.length);
	 		}
		});
	}
	private void StartDisplayThread()
	{
		DispThread = new Thread(new Runnable()	// Create thread object
		{
			public void run()
			{
				for (;!abortflag;)	// Repeat forever until abortflag is set
				{
					if(UdpObj.GetQueueCount() != 0)// Packet has come?
					{	// Yes, some UDP packet has come
						String receivestr;	// Prepare string work
						byte [] senddata = new byte[UdpObj.GetLength()];	// Create receive buffer
						UdpObj.GetData(senddata);	// Get UDP packet from queue
						try
						{
							receivestr = new String(senddata, "US-ASCII");	// Convert byte array to string
							try
							{
								temp = receivestr;
								if(temp.indexOf("MA") !=-1)
									gyairo = temp.split(" ", 0);
							}
							catch(NumberFormatException e){}
							final  String str1 = gyairo[1];
							final  String str2 = gyairo[2];
							final  String str3 = gyairo[3];
							hHandler.post(new Runnable() {
								public void run()    // Send Handler message
								{
								if(flag) {
									text4.setText(str1);    // Post text4 draw command into message queue
									text6.setText(str2);
									text8.setText(str3);
								}
								}
							});
						}
						catch (UnsupportedEncodingException e){}	// Error trap but no error happens
					}
				}// 継続
				abortflag = false;	// Clear abort flag for thread exit indication

			}});
		DispThread.start();	// Start thread just after definition
	}//private
	//
	//	onResume() process
	//
	@Override
	public void onResume()
	{
		super.onResume();	// Do default process
		UdpObj = new Udp(ModuleAdrs, 5556, 5555);	// Create UDP object
		StartDisplayThread();	// Start display temperature & voltage display thread
	}
	//
	//	onPause() process
	//
	@Override
	public void onPause()
	{
		super.onPause();	// Do default process
		if (UdpObj != null)	// UDP object exists?
			UdpObj.Abort();	// Terminate UDP handler thread
		abortflag = true;	// Set display thread abort flag
		while(abortflag);	// Abort confirmation
	}
	//
	// Button RelativeLayout creation
	//
	private RelativeLayout.LayoutParams createParam(int w, int h) {
		return new RelativeLayout.LayoutParams(w, h);
	}
}

Udp.java

import java.io.IOException;
import java.net.*;

public class Udp
{
	//	Common variables
	private DatagramSocket socket;	// Socket work
	private DatagramPacket receivePacket;	// Receive packet object
	private DatagramPacket sendPacket;	// Send packet object
	private InetSocketAddress DestAddress;	// Destination address
	private byte[] recbuf, sendbuf;	// Data buffer
	private byte[][] rec_ring;	// Receive ring buffer
	private int[] rec_port;		// Port number
	private InetAddress[] rec_ip;	// IP address
	private int[] rec_length;	// Data length
	private volatile int queuein, queueout;	// Queue pointer
	private Thread net_thread;	// Thread object
	private volatile boolean abortflag;	// Thread terminate flag
	//
	//	Constructor
	//
	public Udp(String ad, int destport, int srcport)
	{
		try
		{	// Create send data object
			socket = new DatagramSocket(srcport);	// Specify sender port
		}
		catch (SocketException e) {return;}	// exit if fail
		DestAddress = new InetSocketAddress(ad, destport);
  	recbuf = new byte[1500];	// Receive buffer
  	sendbuf = new byte[1500];	// Send buffer
  	rec_ring = new byte[10][1500];	// Receive ring buffer
  	rec_port = new int[10];	// Sender port buffer
  	rec_ip = new InetAddress[10];	// Sender port buffer
  	rec_length = new int[10];	// Length buffer
	  receivePacket = new DatagramPacket(recbuf, recbuf.length);	// Create receive object
	  try
		{	// Create send object
			sendPacket = new DatagramPacket(sendbuf, sendbuf.length, DestAddress);
		}
		catch (SocketException e1) {	}	// Ignore error
		abortflag = false;	// Clear abort flag before thread start
		net_thread = new Thread(new Runnable()	// Create network thread
		{
			public void run()	// Auto run object
			{
				for (;!abortflag;)	// Repeat forever
				{
					if(Receive() == false)	// Try to receive network packet (will be blocked)
						break;
					try
					{
						Thread.sleep(10);	// Sleep
					}
					catch (InterruptedException e){	}// Ignore error(never happens)
				}
				abortflag = false;
			}
		});
		net_thread.start();	// Start network thread
	}
	//
	//	Send packet
	//
	public boolean Send(byte[] data, int len)
	{
		if(sendbuf == null)	// Valid buffer specified?
			return false;	// No then do nothing
		System.arraycopy(data, 0, sendbuf, 0, len);// Make copy of send data
		sendPacket.setLength(len);	// Set data length to send
		try
		{
			socket.send(sendPacket);	// Send data
		}
		catch (SocketException e)	{return false;}	// Error return
		catch (IOException e)	{return false;}	// Error return
		return true;	// Normal return
	}
	//
	//	Receive data and copy to receive queue
	//
	public boolean Receive()
	{
		int queuecou;
		if(socket == null)	// fail to create socket?
			return false;	// Do nothing
		try
		{
			socket.receive(receivePacket);	// Wait until network packet has come
			queuecou = queuein - queueout;	// Get queue available space
			if(queuecou < 0)	// If negative number?
				queuecou += 10;	// Make it plus
			if(queuecou < 9)	// Have space to put?
			{
				System.arraycopy(recbuf, 0, rec_ring[queuein], 0, recbuf.length);	// Save data
				rec_length[queuein] = receivePacket.getLength();	// Save data length
				rec_ip[queuein] = receivePacket.getAddress();	// Save source address
				rec_port[queuein] = receivePacket.getPort();	// Save port number
				if(queuein == 9)	// Increment queue in
					queuein = 0;	// Wrap round
				else
					queuein++;	// Just advance queue entry pointer
			}
		}
		catch (SocketException e) {return false;}// Socket error return
		catch (IOException e) {return false;}		// I/O error return
		return true;	// Normal return
	}
	//
	//	Get queued count
	//
	public int GetQueueCount()
	{
		int queuecou = queuein - queueout;	// Get queue count
		if(queuecou < 0)	// Negative?
			queuecou += 10;	// Make it plus
		return queuecou;	// Return with queued bynber
	}
	//
	//	Get queued data
	//
	public boolean GetData(byte[] result)
	{
		if(queuein != queueout)	// Check queue has data
		{	// Copy queued data
			System.arraycopy(rec_ring[queueout], 0, result, 0, rec_length[queueout]);
			if(queueout == 9)	// Increment queue pointer
				queueout = 0;	// Wrap round
			else
				queueout++;	// Just increment queue pointer
			return true;	// Good result
		}
		return false;	// No data has arrived yet
	}
	//
	//	Get sender port
	//
	public int GetLength()
	{
		if(queuein != queueout)	// Queue has data?
			return rec_length[queueout];	// Return with received data length
		else
			return 0;	// return 0 if no data has arrived
	}
	//
	//	Get sender port
	//
	public int GetPort()
	{
		if(queuein != queueout)	// Queue has data?
			return rec_port[queueout];	// Get sender port number
		else
			return 0;	// return zero if no data has come
	}
	//
	//	Get sender ip address
	//
	public InetAddress GetAddress()
	{
		if(queuein != queueout)		// Queue has data?
			return rec_ip[queueout];	// Get sender IP address number & return
		else
			return null;	// null result if no data is received
	}
	//
	//	Abort receive operation
	//
	public void Abort()
	{
		abortflag = true;	// Set abort flag
		if(socket != null)	// Socket exists?
			socket.close();	// Close socket and force stop receive
		while(abortflag);	// Confirm thread finished successfully
	}
}

WiFi_Udp2.rb

#!mruby
#GR-CITRUS Version 2.19
#ここには全体で使用する変数を定義しています
@Usb = Serial.new(0,115200)
#################################
# I2C L3GD20
#################################
@L3GD20 = 0x6A				    # 0b01101011 ジャイロセンサのアドレス ADDRESS-> 0x6A or 0x6B
@L3GD20_dps = 0.0175	        # 00:0.00875, 10:0.0175, 30:0.07 ジャイロセンサの分解能(deg/s)
@Interval = 500				    # 間隔処理の間隔時間が入る(ms)
@Usb = Serial.new(0, 115200)	#USBシリアル通信の初期化
@Dev = I2c.new(1)				#I2C1 pin0,1を選択
#ESP8266を一度停止させる(リセットと同じ)
pinMode(5,1)
digitalWrite(5,0)   # LOW:Disable
delay 500
digitalWrite(5,1)   # LOW:Disable
if( System.useWiFi() == 0)then
    @Usb.println "WiFi Card can't use."
   System.exit() 
end
@Usb.println "WiFi Ready"
@Usb.println "WiFi Get Version"
@Usb.println WiFi.version
@Usb.println "WiFi disconnect"
@Usb.println WiFi.disconnect
@Usb.println "WiFi connecting"
#Usb.println WiFi.connect("TAROSAY","37000")
@Usb.println WiFi.connect("xxxxxxxxxx","xxxxxxxxxx")
@Usb.println "WiFi ipconfig"
@Usb.println WiFi.ipconfig
@Usb.println "Set wifi mode"
@Usb.println WiFi.setMode 3  #Station-Mode & SoftAPI-Mode
@Usb.println "Enable multiple connection"
@Usb.println WiFi.multiConnect 1
@Usb.println "Create a UDP transmission, for example, id is 4"
#UDP通信,WAMIKAN受信:5555, 送信: 5556
@Usb.println WiFi.udpOpen(4,"192.168.1.6",5555,5556)
#// Motor setup
def set_up()
    #//L Motor PinSET
    pinMode(14, 1)
    pinMode(15, 1)
    #//R Motor PinSET
    pinMode(16, 1)
    pinMode(17, 1)
end
def motor_stop()
    pwm(14, 0)
    pwm(15,0)
    pwm(16, 0)
    pwm(17, 0)
end
def l_motorf()
    pwm(14, 0)
    pwm(15, 255)
end
def l_motorb()
    pwm(14, 255)
    pwm(15, 0)
end
def r_motorf()
    pwm(16, 0)
    pwm(17, 255)
end
def r_motorb()
    pwm(16, 255)
    pwm(17, 0)
end
###################################
# I2CのWrite
###################################
def I2cWrite(id, add, dat)
    @Dev.begin(id)
    @Dev.lwrite(add)
    @Dev.lwrite(dat)
    @Dev.end(1)
end
###################################
# ジャイロセンサの初期化
###################################
def initGyro()
	L3GD20_CTRL_REG1 = 0x20		# Control register
	L3GD20_ENABLE = 0x0F
	L3GD20_CTRL_REG4 = 0x23		# Control register
	L3GD20_CTRL_REG3 = 0x22		# Control register
	L3GD20_RANGE = 0x10			# 00:+-250dps, 10:+-500dps, 30:+-2000dps
	#@L3GD20_dps = 0.0175		# 00:0.00875, 10:0.0175, 30:0.07
	# Turn on all axes, disable power down
	I2cWrite(@L3GD20, L3GD20_CTRL_REG1, L3GD20_ENABLE)
	delay(100)	#100ms待つ
	# +-500dps
	I2cWrite(@L3GD20, L3GD20_CTRL_REG4, L3GD20_RANGE)
	delay(100)	#100ms待つ
end
###################################
# I2CのREAD1
###################################
def I2cRead1(id, addL)
    @Dev.begin(id)
    @Dev.lwrite(addL)
    @Dev.end(1)
    @Dev.request(id, 1)
    dl = @Dev.lread()
    return dl
end
###################################
# I2CのREAD2
###################################
def I2cRead2(id, addL, addH)
    @Dev.begin(id)
    @Dev.lwrite(addL)
    @Dev.end(1)
    @Dev.request(id, 1)
    dl = @Dev.lread()
	@Dev.begin(id)
    @Dev.lwrite(addH)
    @Dev.end(1)
	@Dev.request(id, 1)
    dh = @Dev.lread()
    return dh*256 + dl
end
#// main start
@Usb.println("main")
set_up()
motor_stop()
initGyro()
sw = 1
#UDP 送受信開始
#Usb.println "UDP受信した分がarray配列で返ります"
while (true) do
#500.times do
    
    #角速度を取得します --------------------------------------
	v0 = I2cRead2(@L3GD20, 0x28, 0x29)
	v1 = I2cRead2(@L3GD20, 0x2A, 0x2B)
	v2 = I2cRead2(@L3GD20, 0x2C, 0x2D)
	if v0 > 32767
		v0 = v0 - 65536
	end
	if v1 > 32767
		v1 = v1 - 65536
	end
	if v2 > 32767
		v2 = v2 - 65536
	end
	v0 = (v0 * @L3GD20_dps).round(1)
	v1 = (v1 * @L3GD20_dps).round(1)
	v2 = (v2 * @L3GD20_dps).round(1)
	@Usb.print("gyairo:" + v0.to_s + "," + v1.to_s + "," + v2.to_s + "\n\r")

	#---------------------------------------------------------

    array = WiFi.recv 4   #受信データがない場合は array[0]に -1 が返ります
    if(array[0] >= 0)then
      @Usb.println array[0].to_s
        if(array[0] == 0x66) then #//"f"
            #//WiFi.send 4, "MC11"   
            l_motorf()
            r_motorf()
            @Usb.println("motor FF")
        end
        if(array[0] == 0x62) then #//"b"
            #//WiFi.send 4, "MC21" 
            l_motorb()
            r_motorb()
             @Usb.println("motor back")
        end
       if(array[0] == 0x6C) then #//"l"
            #//WiFi.send 4, "MC31" 
            l_motorb()
            r_motorf()
            @Usb.println("motor left")
        end 
        if(array[0] == 0x72) then #//"r"
            #//WiFi.send 4, "MC41"
            l_motorf()
            r_motorb()
            @Usb.println("motor right")
        end 
        if(array[0] == 0x73) then #//"s"
           #//WiFi.send 4, "MC51"
            motor_stop()
            @Usb.println("motor stop")
        end 
        if(array[0] == 0x6D) then #//"m"
           @Usb.println WiFi.send(4, "MA" + 0x20.chr + v0.to_s + 0x20.chr + v1.to_s + 0x20.chr + v2.to_s + "\r\n").to_s    
        end
         if(array[0] == 0x65) then #//"e"
            @Usb.println WiFi.cClose 4
            @Usb.println WiFi.disconnect
            break
        end
    end 
    #LEDを点滅させます
	led(sw)
	sw = 1 - sw
    delay 100          
end