package com.example.gpsudp3;
import java.io.BufferedReader;

import java.io.BufferedWriter;
import java.io.IOException;
import java.io.InputStreamReader;
import java.io.OutputStreamWriter;
import java.net.DatagramPacket;
import java.net.DatagramSocket;
import java.net.InetAddress;
import java.net.NetworkInterface;
import java.net.ServerSocket;
import java.net.Socket;
import java.net.SocketException;
import java.util.Enumeration;
import com.example.gpsudp3.R;

import ioio.lib.api.AnalogInput;
import ioio.lib.api.DigitalOutput;
import ioio.lib.api.PwmOutput;
import ioio.lib.api.exception.ConnectionLostException;
import ioio.lib.util.AbstractIOIOActivity;
import android.R.string;
import android.app.Activity;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.location.Location;
import android.location.LocationListener;
import android.location.LocationManager;
import android.os.Bundle;
import android.os.Handler;
import android.util.Log;
import android.view.KeyEvent;
import android.widget.TextView;
import java.net.*;
import java.util.StringTokenizer;
import java.util.Timer;
import java.util.TimerTask;

public class MainActivity extends AbstractIOIOActivity implements Runnable,LocationListener,SensorEventListener{
private ServerSocket mServer;
private Socket mSocket;
Handler mHandler = new Handler();
volatile Thread runner = null;
int port = 8090;
TextView t;
private String[] command=new String[5];
private String[] rdata=new String[5]; //LANからの受信データ
private String PreviousCommand=””;
private LocationManager manager = null;
private TextView latitude;
private TextView longitude;
private TextView altitude;
private TextView orientation;
private TextView textcommand;
private String str_latitude;
private String str_longitude;
private String str_altitude;
private String str_orientation;
private ServerSocket server = null;
private Socket socket = null;
private BufferedWriter writer = null;
private BufferedReader in=null;
private SensorManager sensor;
private float DCL=(float) 1.0; //DutyCycle L 左タイヤ
private float DCR=(float) 1.0; //DutyCycle R 右タイヤ
private float DCK=(float)1.0; //DutyCycle Kusakariki
private boolean KusakariFlag=false; //草刈機プロテクタ作動フラグ
float[] orientationValues = new float[2];
final Handler handler=new Handler();
byte[] sendBuffer;
private boolean StopFlag=true;
private double SetDirection=0; //Tank 方位
private int DirectionCount=0; //Direction 平均化用Counter
private double Direction[]=new double[10];
private double AveDirection=0;
private double AveDirectionTank=0;
double VccValue=0; //Vccの読み込み値
String x=””;
private double LatitudeValue; //現在の緯度
private double LongitudeValue; //現在の経度
private boolean StopFlag2=false; //緊急停止フラグ
private int CountDirection=0; //Direction平均化カウント
private double DirectionArray[]={0,0,0,0,0,0,0,0,0,0}; //Direction平均化用
private double SumDirection=0; //Direction平均
private int GoToIKFlag=0; //GoToIKフラグ：0 終了:1 実行中
private int TurnDirectionKFlag=0; //TurnDirection Flag
private double GoToXYx=0; //GoToXYのx
private double GoToXYy=0; //GoToXYのｙ
private double GoToIKi=0; //GoToIKの緯度
private double GoToIKk=0; //GoToIKの経度
private double xd,yd; //畑座標系のx,y
private double xdGoal,ydGoal; //GoToXYのGoal座標
InetSocketAddress remoteAddress =new InetSocketAddress(“192.168.2.26”, 8090);
InetSocketAddress remoteAddress2 =new InetSocketAddress(“192.168.2.27”, 8090);
@SuppressWarnings(“deprecation”)
@Override
//*****************************************************
//* onCreate
//*****************************************************
public void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
setContentView(R.layout.activity_main);
// 以下の１行がGPSをONにする宣言
//manager = (LocationManager)getSystemService(LOCATION_SERVICE);
//緯度・経度・高度変数定義
latitude = (TextView)findViewById(R.id.textView1);
longitude = (TextView)findViewById(R.id.textView2);
altitude = (TextView)findViewById(R.id.textView3);
orientation = (TextView)findViewById(R.id.textView4);
textcommand = (TextView)findViewById(R.id.textView5);
sensor = (SensorManager)getSystemService(SENSOR_SERVICE);
command[0]=””;
command[1]=””;
try{
String ip = MainActivity.getIPAddress();
Thread thread = new Thread(this);
thread.start();
}catch(IOException e){}
if(runner == null){
runner = new Thread(this);
runner.start();
}
TimerTask task=new TimerTask(){ //500ms周期で平均方位を送信
@Override
public void run(){
String msg;
msg=”方位,”+Double.toString((int)(AveDirectionTank*10.+.5)/10.);
SendData(msg);
}
};
Timer timer=new Timer();
timer.schedule(task,0,500);

}

@Override
//*****************************************************
//* UDP/IP送受信
//*****************************************************
public void run() {
UdpIn udpin=new UdpIn();
sleep0(100);
//UdpOut udpout=new UdpOut();
sleep0(100);
udpin.start();
//udpout.start();
}
private static String getIPAddress() throws IOException{
Enumeration interfaces = NetworkInterface.getNetworkInterfaces();

while(interfaces.hasMoreElements()){
NetworkInterface network = interfaces.nextElement();
Enumeration addresses = network.getInetAddresses();

while(addresses.hasMoreElements()){
String address = addresses.nextElement().getHostAddress();

//127.0.0.1と0.0.0.0以外のアドレスが見つかったらそれを返す
if(!”127.0.0.1″.equals(address) && !”0.0.0.0″.equals(address)){
return address;
}
}
}
return “127.0.0.1”;
}
class IOIOThread extends AbstractIOIOActivity.IOIOThread {
private DigitalOutput led;
private DigitalOutput do1;
private DigitalOutput do2;
private DigitalOutput do3;
private DigitalOutput do4;
private DigitalOutput do5;
private DigitalOutput do6;
private DigitalOutput do7;
private DigitalOutput do8;
private PwmOutput pwm3;
private PwmOutput pwm4;
private PwmOutput pwm5;
private PwmOutput pwm6;
private PwmOutput pwmKusa;
private DigitalOutput HeadLight;
private AnalogInput Vcc;
private AnalogInput ain;

@Override
//*************************************************
//* IOIOセットアップ
//*************************************************
protected void setup() throws ConnectionLostException {
int k;
led=ioio_.openDigitalOutput(0,false);
do1=ioio_.openDigitalOutput(1,false);
do2=ioio_.openDigitalOutput(2,false);
pwm3 = ioio_.openPwmOutput(3, 100); // 100hz
pwm4 = ioio_.openPwmOutput(4, 100); // 100hz
pwm5 = ioio_.openPwmOutput(5, 100); // 100hz
pwm6 = ioio_.openPwmOutput(6, 100); // 100hz
do7=ioio_.openDigitalOutput(7,false);
do8=ioio_.openDigitalOutput(8,false);
pwmKusa = ioio_.openPwmOutput(10, 100); // 100hz Kusakariki
HeadLight=ioio_.openDigitalOutput(11,false); // Head Light
pwm3.setDutyCycle((float)0);
pwm4.setDutyCycle((float)0);
pwm5.setDutyCycle((float)0);
pwm6.setDutyCycle((float)0);
pwmKusa.setDutyCycle((float)0);
Vcc=ioio_.openAnalogInput(45);
ain=ioio_.openAnalogInput(46);

}

@Override
//*************************************************
//* IOIO設定ループ
//*************************************************
protected void loop() throws ConnectionLostException {
String msg=””;
String KusakariFlagMsg;
handler.post(new Runnable(){

float suma=0;
int k;
@Override
public void run(){

float a=0;
try {
for(k=1;k<5;k++){
a=ain.read();
suma=suma+a;
}
a=suma/5;
VccValue=Vcc.read();
} catch (InterruptedException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
} catch (ConnectionLostException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
}

if(.08<a){try { //モーター過電流保護
kusakarioff();
KusakariFlag=true;
SendData(“草刈機,NG”);
} catch (ConnectionLostException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
}}

//longitude.setText(“ain=”+Float.toString(a)); //別スレッドからTextViewを書き換え

}

});
if(command[0].equals(“setdirection”)){
SetTankDirection(command[1]);
}
if(command[0].equals(“turndirection”)){
TurnDirection(Double.valueOf(command[1]));
command[0]=””;
}
if(command[0].equals(“setdc”)){
DCL=Float.valueOf(command[1]);
DCR=Float.valueOf(command[1]);
// DCを変更したら、PreviousCommandを再実行する為の処理
command[0]=PreviousCommand;
PreviousCommand=””;
}
if(command[0].equals(“setdcl”)){
DCL=Float.valueOf(command[1]);
// DCを変更したら、PreviousCommandを再実行する為の処理
command[0]=PreviousCommand;
PreviousCommand=””;
}
if(command[0].equals(“setdcr”)){
DCR=Float.valueOf(command[1]);
// DCを変更したら、PreviousCommandを再実行する為の処理
command[0]=PreviousCommand;
PreviousCommand=””;
}
if(command[0].equals(“setdck”)){
DCK=Float.valueOf(command[1]); //草刈DCK変更
// DCKを変更したら、PreviousCommandを再実行する為の処理
command[0]=PreviousCommand;
PreviousCommand=””;
}
if(!command[0].equals(PreviousCommand)){
if(command[0].equals(“forward”)){
tankforward();
}
else if(command[0].equals(“reverse”)){
tankreverse();
}
else if(command[0].equals(“rightturn”)){
tankrightturn();
}
else if(command[0].equals(“leftturn”)){
tankleftturn();
}
else if(command[0].equals(“kusakarion”)){
kusakarion();
sleep0(400);
}
else if(command[0].equals(“kusakarioff”)){
kusakarioff();
}
else if(command[0].equals(“stop”)){
tankstop();
}
else if(command[0].equals(“forwarddirection”)){
TankForwardDirection();
}
else if(command[0].equals(“HeadLightOn”)){
HeadLight.write(true);
}
else if(command[0].equals(“HeadLightOff”)){
HeadLight.write(false);
}
else if(command[0].equals(“movedirection”)){
MoveDirection(Double.valueOf(command[1]),Double.valueOf(command[2]));
}
else if(command[0].equals(“gotoxy”)){
GoToXY(Double.valueOf(command[1]),Double.valueOf(command[2]));
}
else if(command[0].equals(“gotoik2”)){
GoToIK2(Double.valueOf(command[1]),Double.valueOf(command[2]));
}
else if(command[0].equals(“Auto1”)){
Auto1();
}
else if(command[0].equals(“Auto2”)){
Auto2();
}
else if(command[0].equals(“Auto3”)){
Auto3();
}
else if(command[0].equals(“Auto4″)){
Auto4();
}
}
PreviousCommand=command[0];
if (KusakariFlag==true){
KusakariFlagMsg=”true”;
}
else{
KusakariFlagMsg=”false”;
}
}
//*****************************************************
//* tankoff
//*****************************************************
private void tankoff() throws ConnectionLostException{
//tankoff();
do1.write(false);
do2.write(false);
pwm3.setDutyCycle(0);
pwm4.setDutyCycle(0);
pwm5.setDutyCycle(0);
pwm6.setDutyCycle(0);
do7.write(false);
do8.write(false);
sleep0(10);
}
//*****************************************************
//* tankstop
//*****************************************************
private void tankstop() throws ConnectionLostException{
tankoff();
do1.write(false);
do2.write(false);
pwm3.setDutyCycle(1);
pwm4.setDutyCycle(1);
pwm5.setDutyCycle(1);
pwm6.setDutyCycle(1);
do7.write(false);
do8.write(false);
}

//*****************************************************
//* tankforward
//*****************************************************
private void tankforward() throws ConnectionLostException{
tankoff();
do1.write(false);
do2.write(true);
pwm3.setDutyCycle(DCR);
pwm4.setDutyCycle(0);
pwm5.setDutyCycle(DCL);
pwm6.setDutyCycle(0);
do7.write(false);
do8.write(true);
}
//*****************************************************
//* tankreverse
//*****************************************************
private void tankreverse() throws ConnectionLostException{
tankoff();
do1.write(true);
do2.write(false);
pwm3.setDutyCycle(0);
pwm4.setDutyCycle(DCR);
pwm5.setDutyCycle(0);
pwm6.setDutyCycle(DCL);
do7.write(true);
do8.write(false);
}
//*****************************************************
//* tankleftturn
//*****************************************************
private void tankleftturn() throws ConnectionLostException{
tankoff();
do1.write(false);
do2.write(true);
pwm3.setDutyCycle(DCR);
pwm4.setDutyCycle(0);
pwm5.setDutyCycle(0);
pwm6.setDutyCycle(DCL);
do7.write(true);
do8.write(false);
}
//*****************************************************
//* tankrightturn
//*****************************************************
private void tankrightturn() throws ConnectionLostException{
tankoff();
do1.write(true);
do2.write(false);
pwm3.setDutyCycle(0);
pwm4.setDutyCycle(DCR);
pwm5.setDutyCycle(DCL);
pwm6.setDutyCycle(0);
do7.write(false);
do8.write(true);
}
//*****************************************************
//* tankrightturn1:右タイヤブレーキで
//*****************************************************
private void tankrightturn1() throws ConnectionLostException{
tankoff();
//右タイヤ・ブレーキ
do1.write(false);
do2.write(false);
pwm3.setDutyCycle(1);
pwm4.setDutyCycle(1);
//左タイヤ・前進
pwm5.setDutyCycle(DCL);
pwm6.setDutyCycle(0);
do7.write(false);
do8.write(true);
}
//*****************************************************
//* tankleftturn1:左タイヤブレーキで
//*****************************************************
private void tankleftturn1() throws ConnectionLostException{
tankoff();
//右タイヤ前進
do1.write(false);
do2.write(true);
pwm3.setDutyCycle(DCR);
pwm4.setDutyCycle(0);
//左タイヤブレーキ
pwm5.setDutyCycle(1);
pwm6.setDutyCycle(1);
do7.write(false);
do8.write(false);
}
//*****************************************************
//* kusakarion
//*****************************************************
private void kusakarion() throws ConnectionLostException{
pwmKusa.setDutyCycle(DCK);

}
//*****************************************************
//* kusakarioff
//*****************************************************
private void kusakarioff() throws ConnectionLostException{
pwmKusa.setDutyCycle((float)0);
}
//*****************************************************
//* SetTankDirection
//*****************************************************
private void SetTankDirection(String Direction){
SetDirection=Double.valueOf(Direction);
}
//*****************************************************
//* ForwardDirection
//*****************************************************
private void TankForwardDirection() throws ConnectionLostException{
double diff=0;
while(command[0].equals(“forwarddirection”)){
diff=DiffDirection();
//ズレが2度を越えたら修正
if (2<Math.abs(diff)){
if(diff<0){
tankrightturn1();
sleep0(30);
}
else{
tankleftturn1();
sleep0(30);
}
}
else{
tankforward();
}
sleep0(200);
}

}

//*****************************************************
//* TurnDirection:指定の方向へ向く
//*****************************************************
private void TurnDirection(double directionx) throws ConnectionLostException{
double diff=0;
TurnDirectionKFlag=1;
SetDirection=directionx;
diff=DiffDirection();
if(diff<0){
DCL=(float)1.0;
DCR=(float)1.0;
tankrightturn();
while(45<Math.abs(diff) && command[0].equals(“turndirection”)){
diff=DiffDirection();
}
tankstop();
sleep0(20);
while(diff DCL=(float)(Math.abs(diff)/45.); //15度までDC=1.0′
DCR=(float)(Math.abs(diff)/45.);
AdjustDC();
tankrightturn();
sleep0(30);
diff=DiffDirection();
}
}
else if(0<diff){
DCL=(float)1.0;
DCR=(float)1.0;
tankleftturn();
while(45<Math.abs(diff) && command[0].equals(“turndirection”)){
diff=DiffDirection();
}
tankstop();
sleep0(20);
while(0 DCL=(float)(Math.abs(diff)/45.); //15度までDC=1.0′
DCR=(float)(Math.abs(diff)/45.);
AdjustDC();
tankleftturn();
sleep0(30);
diff=DiffDirection();
}
}
tankstop();
DCL=(float)1.0;
DCR=(float)1.0;
TurnDirectionKFlag=0;
}
//*****************************************************
//* TankRightTurnTime:指定時間右回転
//*****************************************************
private void TankRightTurnTime(long t){
try {
tankrightturn();
} catch (ConnectionLostException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
}
sleep0(t);
}
//*****************************************************
//* TankLeftTurnTime：指定時間左回転
//*****************************************************
private void TankLeftTurnTime(long t){
try {
tankleftturn();
} catch (ConnectionLostException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
}
sleep0(t);
}
//*****************************************************
//* AdjustDC:DCR,DCLを0.3～1.0に調整
//*****************************************************
private void AdjustDC(){
if (DCR<0.5){
DCR=(float)0.5;
}
if (1.0<DCR){
DCR=(float)1.0;
}
if(DCL<0.5){
DCL=(float)0.5;
}
if(1.0<DCL){
DCL=(float)1.0;
}
}
//*****************************************************
//* TankStopTime：指定時間停止
//*****************************************************
private void TankStopTime(long t){
try {
tankstop();
} catch (ConnectionLostException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
}
sleep0(t);
}
//*****************************************************
//* TurnDirection2:指定の方向へ向く
//*****************************************************
private void TurnDirection2() throws ConnectionLostException{
double diff=0;
int SleepTime=0;
diff=DiffDirection();
if(diff<0){
while(diff if(10<Math.abs(diff)){
SleepTime=50;
}
else{
SleepTime=30;

}
tankrightturn();
sleep0(SleepTime);
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電流ならば草刈Off
tankstop();
}
diff=DiffDirection();
}
}
else if(0 while(0 if(10<Math.abs(diff)){
SleepTime=50;
}
else{
SleepTime=30;

}
tankleftturn();
sleep0(SleepTime);
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電流ならば草刈Off
tankstop();
}
diff=DiffDirection();
}

}
tankstop();
}
//*****************************************************
//* TurnDirection3:指定の方向へ向く(片側タイヤ停止）
//*****************************************************
private void TurnDirection3() throws ConnectionLostException{
double diff=0;
int SleepTime=0;
diff=DiffDirection();
if(diff<0){
while(diff if(10<Math.abs(diff)){
SleepTime=50;
}
else{
SleepTime=50;

}
tankrightturn1(); //右タイヤブレーキ版
sleep0(SleepTime);
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電流ならば草刈Off
tankstop();
}
diff=DiffDirection();
}
}
else if(0 while(0 if(10<Math.abs(diff)){
SleepTime=50;
}
else{
SleepTime=50;

}
tankleftturn1(); //左タイヤブレーキ版
sleep0(SleepTime);
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電流ならば草刈Off
tankstop();
}
diff=DiffDirection();
}

}
tankstop();
}
//*****************************************************
//* DiffDirection
//*****************************************************
private double DiffDirection(){
double Diff=0;
Diff=AveDirectionTank-SetDirection;
if(180<Diff){
Diff=Diff-360;
}
else if(Diff<-180){
Diff=Diff+360;
}
return Diff;

}
//*****************************************************
//* MoveDirection
//*****************************************************
private void MoveDirection(double direction,double distance){
//方位を指定方向に向ける
SetTankDirection(Double.toString(direction));

try {
//TurnDirection();
//イニシャルの緯度、経度
double i=LatitudeValue;
double k=LongitudeValue;
//Tank前進
tankforward();
while(Distance(k,i,LongitudeValue,LatitudeValue)<distance){
sleep0(10);
}
tankstop();
} catch (ConnectionLostException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
}
}
//******************************
//* GoToXY 　 *
//******************************
private void GoToXY(double x,double y) throws ConnectionLostException{
xdGoal=x; //Goal座標を格納
ydGoal=y; //Goal座標を格納
xdydtoxy(x,y);
xytoik(GoToXYx,GoToXYy);
GoToIK2(GoToIKi,GoToIKk);
}
//******************************
//* xdydtoxy *
//******************************
private void xdydtoxy(double xd, double yd){
double th= 27.7 * Math.PI / 180;
GoToXYx = xd * Math.cos(th) – yd * Math.sin(th);
GoToXYy = xd * Math.sin(th) + yd * Math.cos(th);
}
//******************************
//* xytoik *
//******************************
private void xytoik(double x,double y){
double IPM = (35.952532 – 35.952013) / 57.775;
double KPM = (139.470758 – 139.469752) / 90.654;
GoToIKi = 35.952005 + IPM * y;
GoToIKk = 139.470231 + KPM * x;
}

//********************************************
//* GoToIK2　指定座標へ向かう
//********************************************
private void GoToIK2(double ii2,double kk2) throws ConnectionLostException{
double d,Diff0;
double Ratio = 180 / 4.; //45度もずれたらDCR,Lを0にする。
double itmp,ktmp; //途中の仮目標
double a=1; //a先の目標
GoToIKFlag=1; //GoToIK実行中フラグ
d = Distance(LongitudeValue1.,LatitudeValue1.,kk21., ii21.); //目標座標までの距離
DCR=(float)1.0;
DCL=(float)1.0;
//tankforward();
//command[0]=”turndirection”;
//TurnDirection(GetEarthAngle(LatitudeValue1.0, LongitudeValue1.0, ii21, kk21.));
//Timer1.Start()
tankforward();
command[0]=”gotoik2″;
while (command[0] == “gotoik2” && 0.5 < d){
d = Distance(LongitudeValue1.,LatitudeValue1., kk21., ii21.); //目標座標までの距離
if(a<d){
itmp=a/d(ii2-LatitudeValue)+LatitudeValue; //途中仮目標
ktmp=a/d(kk2-LongitudeValue)+LongitudeValue; // 〃
}
else{
itmp=ii2; //dがa以内になったら、止める
ktmp=kk2;
}

SetDirection = GetEarthAngle(LatitudeValue1., LongitudeValue1., itmp, ktmp);
SetDirection = (int)(SetDirection * 10 + 0.5) / 10.;
Diff0 = DiffDirection();
if(30<Math.abs(Diff0)){
command[0]=(“turndirection”);
TurnDirection(SetDirection);
tankforward();
command[0]=”gotoik2″;
}
else{
if( 0 < Diff0){
DCL =(float)( 0.75 – Diff0 / 180 * Ratio);
DCL =(float)((int)(DCL * 100 + 0.5) / 100.);
if(DCL < 0.2){
DCL =(float)0.;
}
DCR =(float)1.;
}
else{
DCL =(float)1.;
DCR =(float) (1 + Diff0 / 180. * Ratio);
DCR =(float)((int)(DCR * 100 + 0.5) / 100.);
if( DCR < 0.2){
DCR =(float) 0;
}
}
pwm3.setDutyCycle(DCR);
pwm5.setDutyCycle(DCL);
}
//d = Distance(LongitudeValue1.,LatitudeValue1., kk21., ii21.); //目標座標までの距離
handler.post(new Runnable(){
@Override
public void run(){
altitude.setText(Float.toString(DCR)+”:”+Float.toString(DCL));
}
});
DisplayMsg(Double.toString(d));
}

tankstop();
DCR=(float)1.0;
DCL=(float)1.0;
GoToIKFlag=0; //GoToIK2終了フラグセット
}
//*****************************************************
//* Distance
//*****************************************************
private double Distance(double x1,double y1,double x2,double y2){ //k0.i0.k1.i1
double r=6378137; //地球半径
double pi=Math.PI;
x1 = x1 * pi / 180;
y1 = y1 * pi / 180;
x2 = x2 * pi / 180;
y2 = y2 * pi / 180;
double dx = x2 – x1;
double d = r * Math.acos(Math.sin(y1) * Math.sin(y2) + Math.cos(y1) * Math.cos(y2) * Math.cos(dx));
double th = (Math.atan2(Math.sin(dx), Math.cos(y1) * Math.tan(y2) – Math.sin(y1) * Math.cos(dx))) * 180 / pi;
return d;
}
//******************************
//* GetEarthAngle *
//******************************
private double GetEarthAngle(double i0, double k0,double i1,double k1){
double a0;
a0 = GetAngleP0toP1(i0, k0, i1, k1);
if (a0 < 0){
a0 = a0 + 360; //0 to 360に変換
}
a0 = 90 – a0; //90 to -270に変換
if (a0 < 0){
a0 = a0 + 360; // ‘0 to 360に変換
}
return a0;
}
//******************************
//* GetAngleP0toP1 *
//******************************
private double GetAngleP0toP1(double i0,double k0,double i1,double k1){
double dx, dy,a;
dx = k1 – k0;
dy = i1 – i0;
a = Math.sin((90 – i0) * Math.PI / 180);
return Math.atan2(dy, dx * a) * 180 / Math.PI; //主値は±πで計算される
}
//*****************************************************
//* Auto1
//*****************************************************
private void Auto1(){
double CurrentDirection=AveDirection;
boolean ContinueFlag=true;
StopFlag2=false;
while(!StopFlag2){
Auto1Sub(CurrentDirection);
if(KusakariFlag){
try {
SetTankDirection(Double.toString(CurrentDirection));
StopFlag2=false;
KusakariFlag=false;
TurnDirection2();
TankReverseSTime(700);
kusakarion();
sleep0(200);
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電柱ならば草刈Off
StopFlag2=false;
KusakariFlag=false;
TankReverseSTime(800);
kusakarion();
sleep0(200);
CurrentCheck();
}
if (KusakariFlag){
kusakarioff(); //過電柱ならば草刈Off
ContinueFlag=false;
StopFlag2=true;
}
} catch (ConnectionLostException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
} //TurnDirection2

}
}
}
//*****************************************************
//* Auto2
//*****************************************************
private void Auto2(){
double CurrentDirection=AveDirection;
boolean ContinueFlag=true;
CurrentDirection=NormarizeDirection(CurrentDirection);
StopFlag2=false;
while(!StopFlag2){
Auto2Sub(CurrentDirection);
if(KusakariFlag){
try {
SetTankDirection(Double.toString(CurrentDirection));
StopFlag2=false;
KusakariFlag=false;
TurnDirection2();
TankReverseSTime(700);
kusakarion();
sleep0(200);
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電柱ならば草刈Off
StopFlag2=false;
KusakariFlag=false;
TankReverseSTime(800);
kusakarion();
sleep0(200);
CurrentCheck();
}
if (KusakariFlag){
kusakarioff(); //過電柱ならば草刈Off
ContinueFlag=false;
StopFlag2=true;
}
} catch (ConnectionLostException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
} //TurnDirection2

}
}
}
//*****************************************************
//* Auto3
//*****************************************************
private void Auto3(){
double CurrentDirection=AveDirection;
boolean ContinueFlag=true;
StopFlag2=false;
while(!StopFlag2){
Auto3Sub(CurrentDirection);
if(KusakariFlag){
try {
SetTankDirection(Double.toString(CurrentDirection));
StopFlag2=false;
KusakariFlag=false;
TurnDirection2();
TankReverseSTime(700);
kusakarion();
sleep0(200);
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電柱ならば草刈Off
StopFlag2=false;
KusakariFlag=false;
TankReverseSTime(800);
kusakarion();
sleep0(200);
CurrentCheck();
}
if (KusakariFlag){
kusakarioff(); //過電柱ならば草刈Off
ContinueFlag=false;
StopFlag2=true;
}
} catch (ConnectionLostException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
} //TurnDirection2

}
}
}
//*****************************************************
//* Auto4（指定方向で前進しながら首振り）
//*****************************************************
private void Auto4(){
double CurrentDirection=AveDirection;
boolean ContinueFlag=true;
StopFlag2=false;
while(!StopFlag2){
Auto4Sub(CurrentDirection);
if(KusakariFlag){
try {
SetTankDirection(Double.toString(CurrentDirection));
StopFlag2=false;
KusakariFlag=false;
TurnDirection2();
TankReverseSTime(700);
kusakarion();
sleep0(200);
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電柱ならば草刈Off
StopFlag2=false;
KusakariFlag=false;
TankReverseSTime(800);
kusakarion();
sleep0(200);
CurrentCheck();
}
if (KusakariFlag){
kusakarioff(); //過電柱ならば草刈Off
ContinueFlag=false;
StopFlag2=true;
}
} catch (ConnectionLostException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
} //TurnDirection2

}
}
}
//*****************************************************
//* Auto1Sub
//*****************************************************
private void Auto1Sub(double CurrentDirection){
double Direction1=CurrentDirection-45;
double Direction2=CurrentDirection+45;
try {
KusakariFlag=false;
kusakarion();
} catch (ConnectionLostException e1) {
// TODO 自動生成された catch ブロック
e1.printStackTrace();
}
DCL=(float) 0.6;
DCR=(float) 0.6;
while(!StopFlag2 & !KusakariFlag){
try {

SetTankDirection(Double.toString(Direction1));
TurnDirection2(); //TurnDirection2
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電流ならば草刈Off
tankstop();
}
TankForwardSTime(900); //TankForwardSTime
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電流ならば草刈Off
tankstop();
}
SetTankDirection(Double.toString(Direction2));
TurnDirection2(); //TurnDirection2
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電流ならば草刈Off
tankstop();
}
TankForwardSTime(900); //TankForwardSTime
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電流ならば草刈Off
tankstop();
}
} catch (ConnectionLostException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
}
}
try {
tankstop();
} catch (ConnectionLostException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
}
DCL=(float)1.0;
DCR=(float)1.0;
}
//*****************************************************
//* Auto2Sub
//*****************************************************
private void Auto2Sub(double CurrentDirection){
double Direction1=CurrentDirection-45;
double Direction2=CurrentDirection+45;
Direction1=NormarizeDirection(Direction1);
Direction2=NormarizeDirection(Direction2);
try {
KusakariFlag=false;
kusakarion();
sleep0(200);
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電流ならば草刈Off
tankstop();
}
} catch (ConnectionLostException e1) {
// TODO 自動生成された catch ブロック
e1.printStackTrace();
}
while(!StopFlag2 & !KusakariFlag){
try {
SetTankDirection(Double.toString(Direction1));
TurnDirection2(); //TurnDirection2
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電流ならば草刈Off
tankstop();
}
TankForwardSTime(600); //TankForwardSTime
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電流ならば草刈Off
tankstop();
}
SetTankDirection(Double.toString(Direction2));
TurnDirection2(); //TurnDirection2
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電流ならば草刈Off
tankstop();
}
TankForwardSTime(600); //TankForwardSTime
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電流ならば草刈Off
tankstop();
}
} catch (ConnectionLostException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
}
}
try {
tankstop();
} catch (ConnectionLostException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
}
}
//*****************************************************
//* Auto3Sub
//*****************************************************
private void Auto3Sub(double CurrentDirection){
double Direction1=CurrentDirection-45;
double Direction2=CurrentDirection+45;
try {
KusakariFlag=false;
kusakarion();
} catch (ConnectionLostException e1) {
// TODO 自動生成された catch ブロック
e1.printStackTrace();
}
while(!StopFlag2 & !KusakariFlag){
try {
SetTankDirection(Double.toString(Direction1));
TurnDirection3(); //TurnDirection2
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電流ならば草刈Off
tankstop();
}
TankReverseSTime(200); //TankForwardSTime
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電流ならば草刈Off
tankstop();
}
SetTankDirection(Double.toString(Direction2));
TurnDirection3(); //TurnDirection2
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電流ならば草刈Off
tankstop();
}
TankReverseSTime(200); //TankForwardSTime
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電流ならば草刈Off
tankstop();
}
} catch (ConnectionLostException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
}
}
try {
tankstop();
} catch (ConnectionLostException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
}
}
//*****************************************************
//* Auto4Sub
//*****************************************************
private void Auto4Sub(double CurrentDirection){
double Direction1=CurrentDirection-45;
double Direction2=CurrentDirection+45;
Direction1=NormarizeDirection(Direction1);
Direction2=NormarizeDirection(Direction2);
try {
KusakariFlag=false;
kusakarion();
sleep0(200);
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電流ならば草刈Off
tankstop();
}
} catch (ConnectionLostException e1) {
// TODO 自動生成された catch ブロック
e1.printStackTrace();
}
while(!StopFlag2 & !KusakariFlag){
try {
TankForwardSTime(600); //TankForwardSTime
SetTankDirection(Double.toString(Direction1));
TurnDirection2(); //TurnDirection2
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電流ならば草刈Off
tankstop();
}
SetTankDirection(Double.toString(Direction2));
TurnDirection2(); //TurnDirection2
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電流ならば草刈Off
tankstop();
}
SetTankDirection(Double.toString(CurrentDirection));
TurnDirection2(); //TurnDirection2
CurrentCheck();
if (KusakariFlag){
kusakarioff(); //過電流ならば草刈Off
tankstop();
}
} catch (ConnectionLostException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
}
}
try {
tankstop();
} catch (ConnectionLostException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
}
}
//*****************************************************
//* NormarizeDirection
//*****************************************************
private double NormarizeDirection(double Direction){
while(Direction<0){
Direction=Direction+360;
}
while(360<Direction){
Direction=Direction-360;
}
return Direction;

}
//*****************************************************
//* CurrentCheak
//*****************************************************
private void CurrentCheck(){
int k=0;
double a=0;
double suma=0;
for(k=1;k<5;k++){
try {
a=ain.read();
} catch (InterruptedException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
} catch (ConnectionLostException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
}
suma=suma+a;
}
a=suma/5;
if(.08<a){ //モーター過電流保護
KusakariFlag=true;
}

}
//*****************************************************
//* TankForwardSTime
//*****************************************************
private void TankForwardSTime(long STime){
if(!KusakariFlag){
try {
tankforward();
sleep0(STime);
tankstop();
} catch (ConnectionLostException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
}
}
}
//*****************************************************
//* TankReverseSTime
//*****************************************************
private void TankReverseSTime(long STime){
try {
tankreverse();
sleep0(STime);
tankstop();
} catch (ConnectionLostException e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
}
}

}
@Override
//*****************************************************
//* IOIO必須記述
//*****************************************************
protected AbstractIOIOActivity.IOIOThread createIOIOThread() {
return new IOIOThread();
}
@Override
//*****************************************************
//* GPSの為に必要だが、現状意味不明、onなので
// イベントで起動されると思われる
//*****************************************************
protected void onPause() {
// TODO Auto-generated method stub
if(manager != null) {
manager.removeUpdates(this);
}
super.onPause();
}

@Override
//*****************************************************
//* GPSの為必要？　現状意味不明、onなので
//* イベントで起動されると思われる
//*****************************************************
protected void onResume() {
// TODO Auto-generated method stub
if(manager != null) {
manager.requestLocationUpdates(LocationManager.GPS_PROVIDER, 0, 0, this);
}
super.onResume();
sensor.registerListener(this, sensor.getDefaultSensor(Sensor.TYPE_ORIENTATION), SensorManager.SENSOR_DELAY_UI);
}

@Override
//*****************************************************
//* GPSデータが変化したイベントでcallされる
//*****************************************************
public void onLocationChanged(Location location) {
String msg;
String KusakariFlagMsg;
// TODO Auto-generated method stub
//final UdpOut uo = null;
str_latitude = “緯度：” + location.getLatitude();
str_longitude = “経度：” + location.getLongitude();
//str_altitude = “高度：” + location.getAltitude();
LatitudeValue=Double.valueOf(location.getLatitude()); //現在の緯度
LongitudeValue=Double.valueOf(location.getLongitude()); //現在の経度
if (KusakariFlag==true){
KusakariFlagMsg=”true”;
}
else{
KusakariFlagMsg=”false”;
}
msg=”GPS,”+location.getLatitude()+”,”+location.getLongitude()+”,”+location.getAltitude()+”,”+KusakariFlagMsg;
//SendData(msg);
msg=”GPS,”+Double.toString(LatitudeValue)+”,”+Double.toString(LongitudeValue)+”,”+Double.toString(0)+”,”+KusakariFlagMsg;
//SendData(msg);
msg=”方位,”+Double.toString(AveDirection);
//SendData(msg);
//VccValueを送信
msg=”Vcc,”+Double.toString(VccValue);
SendData(msg);
//この方法で得られる方位は、移動することによってそのベクトルから得ている様である。
//str = “方位：” + location.getBearing();
//orientation.setText(str);
handler.post(new Runnable(){
@Override
public void run(){
latitude.setText(str_latitude);
longitude.setText(str_longitude);
altitude.setText(str_altitude);
}
});

}
//***********************************************************
//* SendData
//***********************************************************
public void SendData(String SData){
//sendBuffer =str_longitude.getBytes();
sendBuffer=(“”+SData).getBytes();
try{
DatagramPacket sendPacket =new DatagramPacket(sendBuffer, sendBuffer.length, remoteAddress);
DatagramPacket sendPacket2 =new DatagramPacket(sendBuffer, sendBuffer.length, remoteAddress2);
new DatagramSocket().send(sendPacket);
new DatagramSocket().send(sendPacket2);
}catch(IOException ex){Log.e(ex.getClass().getName(), ex.getMessage());}
}
@Override
public void onProviderDisabled(String provider) {
// TODO Auto-generated method stub
}

@Override
public void onProviderEnabled(String provider) {
// TODO Auto-generated method stub
}

@Override
public void onStatusChanged(String provider, int status, Bundle extras) {
// TODO Auto-generated method stub
}
//*************************************************
//* sleep
//*************************************************
void sleep0(long t){
try {
Thread.sleep(t);
} catch (InterruptedException e) {
}
}
@Override
//*************************************************
//* onSensorChange
//*************************************************
public void onSensorChanged(SensorEvent event) {
String msg;
double DirectionValue=0;
int N=5;
double SumDirectionTmp,AveDirectionTmp;
int k;
switch (event.sensor.getType()) {
case Sensor.TYPE_ORIENTATION:
orientationValues = event.values.clone();
DirectionValue=orientationValues[0];
if(CountDirection==N){
CountDirection=0;
}
if(350 SumDirectionTmp=SumDirection-DirectionArray[CountDirection]+DirectionValue+360.;
AveDirectionTmp=SumDirectionTmp/N;
if(AveDirectionTmp<360){
AveDirection=AveDirectionTmp;
DirectionArray[CountDirection]=DirectionValue+360.;
SumDirection=SumDirectionTmp;
}
else{
SumDirection=0;
for(k=0;k<N;k++){
DirectionArray[k]=DirectionArray[k]-360.;
SumDirection=SumDirection+DirectionArray[k];
}
SumDirection=SumDirection-DirectionArray[CountDirection]+DirectionValue;
DirectionArray[CountDirection]=DirectionValue;
AveDirection=SumDirection/N;
}
}
else if(0 SumDirectionTmp=SumDirection-DirectionArray[CountDirection]+DirectionValue-360.;
AveDirectionTmp=SumDirectionTmp/N;
if(0<AveDirectionTmp){
AveDirection=AveDirectionTmp;
DirectionArray[CountDirection]=DirectionValue-360.;
SumDirection=SumDirectionTmp;
}
else{
SumDirection=0;
for(k=0;k<N;k++){
DirectionArray[k]=DirectionArray[k]+360.;
SumDirection=SumDirection+DirectionArray[k];
}
SumDirection=SumDirection-DirectionArray[CountDirection]+DirectionValue;
DirectionArray[CountDirection]=DirectionValue;
AveDirection=SumDirection/N;
}
}
else{
SumDirection=SumDirection-DirectionArray[CountDirection]+DirectionValue;
DirectionArray[CountDirection]=DirectionValue;
AveDirection=SumDirection/N;
}
CountDirection=CountDirection+1;
}
AveDirectionTank=AveDirection+90-0; //スマートフォンの向きTankの向き調整
if(360<AveDirectionTank){
AveDirectionTank=AveDirectionTank-360;
}
str_orientation=”方位:”+Double.toString(AveDirectionTank);
//msg=”方位,”+Double.toString((int)(AveDirectionTank10.+.5)/10.);
//SendData(msg);
handler.post(new Runnable(){
@Override
public void run(){
orientation.setText(“方位:”+Double.toString((int)(AveDirectionTank10.+.5)/10.));
}
});
}
@Override
public void onAccuracyChanged(Sensor arg0, int arg1) {
// TODO 自動生成されたメソッド・スタブ

}
//*************************************************
//* UdpIn
//*************************************************
class UdpIn extends Thread{
public void run(){
//BufferedReader br = new BufferedReader( new InputStreamReader(System.in) );
//SocketAddress sockAddress;//接続してきた送信元
// 受け付けるデータバッファとUDPパケットを作成
try{
DatagramSocket recSocket =new DatagramSocket(8090);
byte []buf = new byte[256];
DatagramPacket packet= new DatagramPacket(buf,buf.length);
String msg=””;
int len;
while(true){
recSocket.receive(packet);
len = packet.getLength();//受信バイト数取得
msg =new String(buf, 0, len);
DisplayMsg(“Msg=”+msg);
SplitCsv(msg);
if(rdata[0].equals(“stop”)){
StopFlag2=true;
}
if(rdata[0].equals(“GPS”)){
LatitudeValue=Double.valueOf(rdata[1]);
LongitudeValue=Double.valueOf(rdata[2]);
iktoxdyd(LatitudeValue,LongitudeValue);
handler.post(new Runnable(){
@Override
public void run(){
latitude.setText(“緯度:”+Double.toString((int)(LatitudeValue1000000.+.5)/1000000.));
longitude.setText(“経度:”+Double.toString((int)(LongitudeValue1000000.+.5)/1000000.));
}
});
}
else if(rdata[0].equals(“SendStatus”)){
SendStatus();
}
else{
command[0]=rdata[0];
command[1]=rdata[1];
command[2]=rdata[2];
command[3]=rdata[3];
}
}
//recSocket=null;
}catch (IOException e) {DisplayError(“Tryの中”);}
}
}
//******************************
//* iktoxdyd *
//******************************
private void iktoxdyd(double i,double k){
double x,y;
double IPM= (.****** – **.******) / 57.775;           //基準点による緯度計算  敢えてにしました
double KPM= (.****** – .******) / 90.654;    //基準点による経度計算
double th = -27.7 * Math.PI / 180.;
y=(i-.******)/IPM;
x=(k-*********)/KPM;
xd=xMath.cos(th)-yMath.sin(th);
yd=xMath.sin(th)+yMath.cos(th);
}
//*****************************************************
// SendStatus
//*****************************************************
private void SendStatus(){
String msg=””;
msg=”Status,”+Integer.toString(GoToIKFlag)+”,”+Integer.toString(TurnDirectionKFlag);
SendData(msg);
}
//*************************************************
//* SplitCsv
//*************************************************
private void SplitCsv(String Input){
int n=0;
StringTokenizer st = new StringTokenizer(Input, “,”);
while (st.hasMoreTokens()) {
rdata[n]=st.nextToken();
n=n+1;
}
}

//*************************************************
//* UdpOut
//*************************************************
/*
class UdpOut extends Thread{
public void run(){
while(StopFlag){
sleep0(500);
SendData(str_latitude);
SendData(str_longitude);
SendData(str_altitude);
SendData(str_orientation);
}
}
public void SendData(String SData){
//sendBuffer =str_longitude.getBytes();
sendBuffer=(“”+SData).getBytes();
try{
DatagramPacket sendPacket =new DatagramPacket(sendBuffer, sendBuffer.length, remoteAddress);
new DatagramSocket().send(sendPacket);
}catch(IOException ex){Log.e(ex.getClass().getName(), ex.getMessage());}
}
}
/
//*************************************************
// onKeyDown
//*************************************************
@Override
public boolean onKeyDown(int keyCode, KeyEvent event) {
// BackBtnアクション
if(keyCode==KeyEvent.KEYCODE_BACK){
StopFlag=false;
System.exit(0);
}
return false;
}

//*************************************************
//* Disiplay Error
//*************************************************
public void DisplayError(final String err){
handler.post(new Runnable(){
@Override
public void run(){
altitude.setText(“Error:”+err);
//System.exit(0);
}
});

}
//*************************************************
//* Disiplay Error
//*************************************************
public void DisplayMsg(final String msg){
handler.post(new Runnable(){
@Override
public void run(){
textcommand.setText(msg);
}
});

}

}