●ドラッグコントロールを追加(Android)--- オプション
前回開発したAndroidアプリに、画面をドラッグして歩行をコントロールする機能を追加します。右手と左手の親指をロボットの両足に見立てて画面上を歩くような動作で操縦できます。
こちらのテーマは内容も複雑でコードの分量も増えますのでオプション(番外編)とします。
まずスマホ画面に、ドラッグコントロールモードにするためのボタンと指の動きを拾うための部品(ImageView)を追加します。activity_main.xmlを変更します。
<?xml version="1.0" encoding="utf-8"?> <LinearLayout xmlns:android="http://schemas.android.com/apk/res/android" xmlns:app="http://schemas.android.com/apk/res-auto" android:layout_width="match_parent" android:layout_height="match_parent" android:layout_margin="5dp" android:orientation="vertical"> <LinearLayout android:layout_width="match_parent" android:layout_height="wrap_content" android:layout_margin="2dp" android:orientation="horizontal"> <Button android:id="@+id/button_scan" android:layout_width="wrap_content" android:layout_height="wrap_content" android:text="Scan" android:textSize="15sp"/> <TextView android:layout_width="wrap_content" android:layout_height="wrap_content" android:text="Device name" android:textSize="15sp"/> <Space android:layout_width="5dp" android:layout_height="wrap_content"/> <TextView android:id="@+id/textview_devicename" android:layout_width="match_parent" android:layout_height="wrap_content" android:textSize="15sp"/> </LinearLayout> <Space android:layout_width="match_parent" android:layout_height="5dp"/> <LinearLayout android:layout_width="match_parent" android:layout_height="wrap_content" android:layout_margin="2dp" android:orientation="horizontal"> <Button android:id="@+id/button_connect" android:layout_width="wrap_content" android:layout_height="wrap_content" android:text="connect" android:textSize="15sp"/> <Space android:layout_width="5dp" android:layout_height="wrap_content"/> <Button android:id="@+id/button_disconnect" android:layout_width="wrap_content" android:layout_height="wrap_content" android:text="disconnect" android:textSize="15sp"/> </LinearLayout> <Space android:layout_width="match_parent" android:layout_height="5dp"/> <LinearLayout android:layout_width="match_parent" android:layout_height="wrap_content" android:layout_margin="2dp" android:orientation="horizontal"> <Space android:layout_width="5dp" android:layout_height="wrap_content"/> <Button android:id="@+id/button_LTRN" android:layout_width="wrap_content" android:layout_height="wrap_content" android:text="LTRN" android:textSize="15sp"/> <Space android:layout_width="5dp" android:layout_height="wrap_content"/> <Button android:id="@+id/button_FWRD" android:layout_width="wrap_content" android:layout_height="wrap_content" android:text="FWRD" android:textSize="15sp"/> <Space android:layout_width="5dp" android:layout_height="wrap_content"/> <Button android:id="@+id/button_RTRN" android:layout_width="wrap_content" android:layout_height="wrap_content" android:text="RTRN" android:textSize="15sp"/> <Space android:layout_width="5dp" android:layout_height="wrap_content"/> <Button android:id="@+id/button_SND" android:layout_width="wrap_content" android:layout_height="wrap_content" android:text="SND" android:textSize="15sp"/> </LinearLayout> <LinearLayout android:layout_width="match_parent" android:layout_height="wrap_content" android:layout_margin="2dp" android:orientation="horizontal"> <Space android:layout_width="5dp" android:layout_height="wrap_content"/> <Button android:id="@+id/button_LEFT" android:layout_width="wrap_content" android:layout_height="wrap_content" android:text="LEFT" android:textSize="15sp"/> <Space android:layout_width="5dp" android:layout_height="wrap_content"/> <Button android:id="@+id/button_STOP" android:layout_width="wrap_content" android:layout_height="wrap_content" android:text="STOP" android:textSize="15sp"/> <Space android:layout_width="5dp" android:layout_height="wrap_content"/> <Button android:id="@+id/button_RGHT" android:layout_width="wrap_content" android:layout_height="wrap_content" android:text="RGHT" android:textSize="15sp"/> <Space android:layout_width="5dp" android:layout_height="wrap_content"/> <Button android:id="@+id/button_WALK" android:layout_width="wrap_content" android:layout_height="wrap_content" android:text="WALK" android:textSize="15sp"/> </LinearLayout> <LinearLayout android:layout_width="match_parent" android:layout_height="wrap_content" android:layout_margin="2dp" android:orientation="horizontal"> <Space android:layout_width="5dp" android:layout_height="wrap_content"/> <Button android:id="@+id/button_LED_ON" android:layout_width="wrap_content" android:layout_height="wrap_content" android:text="LED_ON" android:textSize="15sp"/> <Space android:layout_width="5dp" android:layout_height="wrap_content"/> <Button android:id="@+id/button_BWRD" android:layout_width="wrap_content" android:layout_height="wrap_content" android:text="BWRD" android:textSize="15sp"/> <Space android:layout_width="5dp" android:layout_height="wrap_content"/> <Button android:id="@+id/button_LED_OFF" android:layout_width="wrap_content" android:layout_height="wrap_content" android:text="LED_OFF" android:textSize="15sp"/> </LinearLayout> <ImageView android:id="@+id/imageView1" android:layout_width="match_parent" android:layout_height="match_parent" app:srcCompat="@color/colorPrimary" /> </LinearLayout>
次にMainActivity.javaの変更箇所を示します。
package jp.co.meuse.robotcontroller; import android.annotation.TargetApi; import android.app.Activity; import android.bluetooth.BluetoothAdapter; import android.bluetooth.BluetoothDevice; import android.bluetooth.BluetoothGatt; import android.bluetooth.BluetoothGattCallback; import android.bluetooth.BluetoothGattCharacteristic; import android.bluetooth.BluetoothGattDescriptor; import android.bluetooth.BluetoothGattService; import android.bluetooth.BluetoothManager; import android.bluetooth.BluetoothProfile; import android.bluetooth.le.ScanCallback; import android.bluetooth.le.ScanResult; import android.content.Context; import android.content.Intent; import android.content.pm.PackageManager; import android.graphics.PointF; import android.os.Build; import android.support.v7.app.AppCompatActivity; import android.os.Bundle; import android.util.Log; import android.view.Menu; import android.view.MenuItem; import android.view.MotionEvent; import android.view.View; import android.widget.Button; import android.widget.CheckBox; import android.widget.ImageView; import android.widget.TextView; import android.widget.Toast; import java.util.HashMap; import java.util.UUID; public class MainActivity extends AppCompatActivity implements View.OnClickListener,View.OnTouchListener{ private static final UUID UUID_SERVICE_PRIVATE = UUID.fromString( "4fafc201-1fb5-459e-8fcc-c5c9c331914b" ); private static final UUID UUID_CHARACTERISTIC_PRIVATE1 = UUID.fromString( "beb5483e-36e1-4688-b7f5-ea07361b26a8" ); private static final int REQUEST_ENABLEBLUETOOTH = 1; private BluetoothAdapter mBluetoothAdapter; private String mDeviceAddress = ""; private BluetoothGatt mBluetoothGatt = null; // GUIs private Button mButton_Scan; // スキャン private Button mButton_Connect; // 接続 private Button mButton_Disconnect;// 切断 private Button mButton_LTRN; // 左旋回 private Button mButton_FWRD; // 前進 private Button mButton_RTRN; // 右旋回 private Button mButton_LEFT; // 左横歩き private Button mButton_STOP; // 停止 private Button mButton_RGHT; // 右横歩き private Button mButton_LED_ON; // LED on private Button mButton_BWRD; // 後退 private Button mButton_LED_OFF; // LED off private Button mButton_SND; // 音声再生 private Button mButton_WALK; // 歩行制御 private Button mButton_ARM; // 歩行制御 private ImageView mImageView; // ドラッグ // Drag mode public static final int NORMAL = 0; public static final int WALK = 10; public static final int ARM = 20; public static final int FwdBack = 1; public static final int LeftTurn = 2; public static final int LeftStep = 3; public static final int RightTurn = 4; public static final int RightStep = 5; private int mode =WALK; private int w_mode; private boolean w_phase; PointF[] pos; private HashMap<String,PointF> points = new HashMap<String,PointF>(); private final BluetoothGattCallback mGattcallback = new BluetoothGattCallback() { @Override public void onConnectionStateChange( BluetoothGatt gatt, int status, int newState ) { if( BluetoothGatt.GATT_SUCCESS != status ) { return; } if( BluetoothProfile.STATE_CONNECTED == newState ) { mBluetoothGatt.discoverServices(); runOnUiThread( new Runnable() { public void run() { mButton_Connect.setEnabled(false ); mButton_Disconnect.setEnabled( true ); mButton_LTRN.setEnabled( true ); mButton_FWRD.setEnabled( true ); mButton_RTRN.setEnabled( true ); mButton_LEFT.setEnabled( true ); mButton_STOP.setEnabled( true ); mButton_RGHT.setEnabled( true ); mButton_LED_ON.setEnabled( true ); mButton_BWRD.setEnabled( true ); mButton_LED_OFF.setEnabled( true ); mButton_SND.setEnabled( true ); mButton_WALK.setEnabled( true ); mButton_ARM.setEnabled( true ); mImageView.setEnabled( true ); } } ); return; } if( BluetoothProfile.STATE_DISCONNECTED == newState ) { mBluetoothGatt.connect(); runOnUiThread( new Runnable() { public void run() { } } ); return; } } @Override public void onServicesDiscovered( BluetoothGatt gatt, int status ) { if( BluetoothGatt.GATT_SUCCESS != status ) { return; } for( BluetoothGattService service : gatt.getServices() ) { if( ( null == service ) || ( null == service.getUuid() ) ) { continue; } if( UUID_SERVICE_PRIVATE.equals( service.getUuid() ) ) { runOnUiThread( new Runnable() { public void run() { mButton_LTRN.setEnabled( true ); mButton_FWRD.setEnabled( true ); mButton_RTRN.setEnabled( true ); mButton_LEFT.setEnabled( true ); mButton_STOP.setEnabled( true ); mButton_RGHT.setEnabled( true ); mButton_LED_ON.setEnabled( true ); mButton_BWRD.setEnabled( true ); mButton_LED_OFF.setEnabled( true ); mButton_SND.setEnabled( true ); mButton_WALK.setEnabled( true ); mButton_ARM.setEnabled( true ); mImageView.setEnabled( true ); } } ); continue; } } } @Override public void onCharacteristicWrite( BluetoothGatt gatt, BluetoothGattCharacteristic characteristic, int status ) { if( BluetoothGatt.GATT_SUCCESS != status ) { return; } if( UUID_CHARACTERISTIC_PRIVATE1.equals( characteristic.getUuid() ) ) { runOnUiThread( new Runnable() { public void run() { mButton_LTRN.setEnabled( true ); mButton_FWRD.setEnabled( true ); mButton_RTRN.setEnabled( true ); mButton_LEFT.setEnabled( true ); mButton_STOP.setEnabled( true ); mButton_RGHT.setEnabled( true ); mButton_LED_ON.setEnabled( true ); mButton_BWRD.setEnabled( true ); mButton_LED_OFF.setEnabled( true ); mButton_SND.setEnabled( true ); mButton_WALK.setEnabled( true ); mButton_ARM.setEnabled( true ); mImageView.setEnabled( true ); } } ); return; } } }; @Override protected void onCreate(Bundle savedInstanceState) { super.onCreate(savedInstanceState); setContentView(R.layout.activity_main); mButton_Scan = (Button)findViewById( R.id.button_scan ); mButton_Scan.setOnClickListener( this ); mButton_Connect = (Button)findViewById( R.id.button_connect ); mButton_Connect.setOnClickListener( this ); mButton_Disconnect = (Button)findViewById( R.id.button_disconnect ); mButton_Disconnect.setOnClickListener( this ); mButton_LTRN = (Button)findViewById( R.id.button_LTRN ); mButton_LTRN.setOnClickListener( this ); mButton_FWRD = (Button)findViewById( R.id.button_FWRD ); mButton_FWRD.setOnClickListener( this ); mButton_RTRN = (Button)findViewById( R.id.button_RTRN ); mButton_RTRN.setOnClickListener( this ); mButton_LEFT = (Button)findViewById( R.id.button_LEFT ); mButton_LEFT.setOnClickListener( this ); mButton_STOP = (Button)findViewById( R.id.button_STOP ); mButton_STOP.setOnClickListener( this ); mButton_RGHT = (Button)findViewById( R.id.button_RGHT ); mButton_RGHT.setOnClickListener( this ); mButton_LED_ON = (Button)findViewById( R.id.button_LED_ON ); mButton_LED_ON.setOnClickListener( this ); mButton_BWRD = (Button)findViewById( R.id.button_BWRD ); mButton_BWRD.setOnClickListener( this ); mButton_LED_OFF = (Button)findViewById( R.id.button_LED_OFF ); mButton_LED_OFF.setOnClickListener( this ); mButton_SND = (Button)findViewById( R.id.button_SND ); mButton_SND.setOnClickListener( this ); mButton_WALK = (Button)findViewById( R.id.button_WALK ); mButton_WALK.setOnClickListener( this ); mButton_ARM = (Button)findViewById( R.id.button_ARM ); mButton_ARM.setOnClickListener( this ); mImageView = (ImageView)findViewById(R.id.imageView1); mImageView.setOnTouchListener(this ); pos = new PointF[2]; if( !getPackageManager().hasSystemFeature( PackageManager.FEATURE_BLUETOOTH_LE ) ) { Toast.makeText( this, R.string.ble_is_not_supported, Toast.LENGTH_SHORT ).show(); finish(); return; } BluetoothManager bluetoothManager = (BluetoothManager)getSystemService( Context.BLUETOOTH_SERVICE ); mBluetoothAdapter = bluetoothManager.getAdapter(); if( null == mBluetoothAdapter ) { Toast.makeText( this, R.string.bluetooth_is_not_supported, Toast.LENGTH_SHORT ).show(); finish(); return; } } private void scanNewDevice() { // OS ver.5.0以上ならBluetoothLeScannerを使用する. if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.LOLLIPOP) { this.startScanByBleScanner(); } else { } } private void startScanByBleScanner() { final android.bluetooth.le.BluetoothLeScanner scanner = mBluetoothAdapter.getBluetoothLeScanner(); scanner.startScan(new ScanCallback() { @Override public void onScanResult(int callbackType, ScanResult result) { super.onScanResult(callbackType, result); if(result.getDevice().getName() != null) { if (result.getDevice().getName().equals("MyESP32")) { mButton_Connect.setEnabled(true); mButton_Disconnect.setEnabled(false); mDeviceAddress = result.getDevice().getAddress(); ( (TextView)findViewById( R.id.textview_devicename ) ).setText( "MyESP32" ); connect(); } } } @Override public void onScanFailed(int intErrorCode) { super.onScanFailed(intErrorCode); } }); } @Override protected void onResume() { super.onResume(); requestBluetoothFeature(); mButton_Connect.setEnabled( false ); mButton_Disconnect.setEnabled( false ); mButton_LTRN.setEnabled( false ); mButton_FWRD.setEnabled( false ); mButton_RTRN.setEnabled( false ); mButton_LEFT.setEnabled( false ); mButton_STOP.setEnabled( false ); mButton_RGHT.setEnabled( false ); mButton_LED_ON.setEnabled( false ); mButton_BWRD.setEnabled( false ); mButton_LED_OFF.setEnabled( false ); mButton_SND.setEnabled( false ); mButton_WALK.setEnabled( false ); mButton_ARM.setEnabled( false ); mImageView.setEnabled( false ); if( !mDeviceAddress.equals( "" ) ) { mButton_Connect.setEnabled( true ); } mButton_Connect.callOnClick(); } @Override protected void onPause() { super.onPause(); disconnect(); } @Override protected void onDestroy() { super.onDestroy(); if( null != mBluetoothGatt ) { mBluetoothGatt.close(); mBluetoothGatt = null; } } private void requestBluetoothFeature() { if( mBluetoothAdapter.isEnabled() ) { return; } Intent enableBtIntent = new Intent( BluetoothAdapter.ACTION_REQUEST_ENABLE ); startActivityForResult( enableBtIntent, REQUEST_ENABLEBLUETOOTH ); } @Override protected void onActivityResult( int requestCode, int resultCode, Intent data ) { switch( requestCode ) { case REQUEST_ENABLEBLUETOOTH: if( Activity.RESULT_CANCELED == resultCode ) { Toast.makeText( this, R.string.bluetooth_is_not_working, Toast.LENGTH_SHORT ).show(); finish(); return; } break; } super.onActivityResult( requestCode, resultCode, data ); } @Override public void onClick( View v ) { byte[] command = new byte[1]; if( mButton_WALK.getId() == v.getId() ) { mButton_WALK.setEnabled( false ); mode = WALK; return; }else if( mButton_ARM.getId() == v.getId() ) { mButton_ARM.setEnabled(false); mode = ARM; return; }else { mode = NORMAL; if (mButton_Scan.getId() == v.getId()) { scanNewDevice(); return; } if (mButton_Connect.getId() == v.getId()) { mButton_Connect.setEnabled(false); connect(); return; } if (mButton_Disconnect.getId() == v.getId()) { mButton_Disconnect.setEnabled(false); disconnect(); return; } if (mButton_LTRN.getId() == v.getId()) { mButton_LTRN.setEnabled(false); command[0] = 1; writeCharacteristic(UUID_SERVICE_PRIVATE, UUID_CHARACTERISTIC_PRIVATE1, command); return; } if (mButton_FWRD.getId() == v.getId()) { mButton_FWRD.setEnabled(false); command[0] = 2; writeCharacteristic(UUID_SERVICE_PRIVATE, UUID_CHARACTERISTIC_PRIVATE1, command); return; } if (mButton_RTRN.getId() == v.getId()) { mButton_RTRN.setEnabled(false); command[0] = 3; writeCharacteristic(UUID_SERVICE_PRIVATE, UUID_CHARACTERISTIC_PRIVATE1, command); return; } if (mButton_LEFT.getId() == v.getId()) { mButton_LEFT.setEnabled(false); command[0] = 4; writeCharacteristic(UUID_SERVICE_PRIVATE, UUID_CHARACTERISTIC_PRIVATE1, command); return; } if (mButton_STOP.getId() == v.getId()) { mButton_STOP.setEnabled(false); command[0] = 5; writeCharacteristic(UUID_SERVICE_PRIVATE, UUID_CHARACTERISTIC_PRIVATE1, command); return; } if (mButton_RGHT.getId() == v.getId()) { mButton_RGHT.setEnabled(false); command[0] = 6; writeCharacteristic(UUID_SERVICE_PRIVATE, UUID_CHARACTERISTIC_PRIVATE1, command); return; } if (mButton_LED_ON.getId() == v.getId()) { mButton_LED_ON.setEnabled(false); command[0] = 7; writeCharacteristic(UUID_SERVICE_PRIVATE, UUID_CHARACTERISTIC_PRIVATE1, command); return; } if (mButton_BWRD.getId() == v.getId()) { mButton_BWRD.setEnabled(false); command[0] = 8; writeCharacteristic(UUID_SERVICE_PRIVATE, UUID_CHARACTERISTIC_PRIVATE1, command); return; } if (mButton_LED_OFF.getId() == v.getId()) { mButton_LED_OFF.setEnabled(false); command[0] = 9; writeCharacteristic(UUID_SERVICE_PRIVATE, UUID_CHARACTERISTIC_PRIVATE1, command); return; } if (mButton_SND.getId() == v.getId()) { mButton_SND.setEnabled(false); command[0] = 0; writeCharacteristic(UUID_SERVICE_PRIVATE, UUID_CHARACTERISTIC_PRIVATE1, command); return; } } } @Override public boolean onTouch( View v ,MotionEvent event) { int width = v.getWidth(); int height = v.getHeight(); int count = event.getPointerCount(); int index = event.getActionIndex(); int pointerID = event.getPointerId(index); byte[] command = new byte[5]; switch (event.getActionMasked()) { case MotionEvent.ACTION_DOWN: case MotionEvent.ACTION_POINTER_DOWN: points.put(""+pointerID, new PointF(event.getX(), event.getY())); if(mode == WALK){ for(int i=0; i<count; i++) { pos[i] = points.get("" + event.getPointerId(i)); pos[i].x = event.getX(i); pos[i].y = event.getY(i); } if (pos[0].y > height * 0.4 && pos[0].y < height * 0.6) { if(count==2) { if (pos[0].x > width / 2) { if (pos[1].y > height * 0.4 && pos[1].y < height * 0.6) { w_mode = RightStep; } else { w_mode = RightTurn; } } else { if (pos[1].y > height * 0.4 && pos[1].y < height * 0.6) { w_mode = LeftStep; } else { w_mode = LeftTurn; } } w_phase = !w_phase; } }else{ w_mode = FwdBack; } }else if(mode==ARM){ command[0] = 122; command[1] = command[3] = 40; command[2] = command[4] = 13; for ( int i = 0; i < count; i++ ) { pos[i] = points.get("" + event.getPointerId(i)); pos[i].x = event.getX(i); pos[i].y = event.getY(i); points.put(""+pointerID, new PointF(event.getX(i), event.getY(i))); } for (int i = 0; i < count; i++){ if(i==0){ if(pos[0].x > width/2){ if(pos[0].x < width * 0.6){ command[1] = 0; }else if(pos[0].x > width * 0.9){ command[1] = 90; }else{ command[1] = (byte) (90 * (pos[0].x - width * 0.6) / (width * 0.3)); } if(pos[0].y < height * 0.2){ command[2] = 90; }else if(pos[0].y > height * 0.8){ command[2] = 0; }else{ command[2] = (byte) (90 - 90 * (pos[0].y - height * 0.2) / (height * 0.6)); } }else{ if(pos[0].x < width * 0.1){ command[3] = 90; }else if(pos[0].x > width * 0.4){ command[3] = 0; }else{ command[3] = (byte) (90 * (width * 0.4 - pos[0].x) / (width * 0.3)); } if(pos[0].y < height * 0.2){ command[4] = 90; }else if(pos[0].y > height * 0.8){ command[4] = 0; }else{ command[4] = (byte) (90 - 90 * (pos[0].y - height * 0.2) / (height * 0.6)); } } } if(i==1){ if(pos[1].x > width/2){ if(pos[1].x < width * 0.6){ command[1] = 0; }else if(pos[1].x > width * 0.9){ command[1] = 90; }else{ command[1] = (byte) (90 * (pos[1].x - width * 0.6) / (width * 0.3)); } if(pos[1].y < height * 0.2){ command[2] = 90; }else if(pos[1].y > height * 0.8){ command[2] = 0; }else{ command[2] = (byte) (90 - 90 * (pos[1].y - height * 0.2) / (height * 0.6)); } }else{ if(pos[1].x < width * 0.1){ command[3] = 90; }else if(pos[1].x > width * 0.4){ command[3] = 0; }else{ command[3] = (byte) (90 * (width * 0.4 - pos[1].x) / (width * 0.3)); } if(pos[1].y < height * 0.2){ command[4] = 90; }else if(pos[1].y > height * 0.8){ command[4] = 0; }else{ command[4] = (byte) (90 - 90 * (pos[1].y - height * 0.2) / (height * 0.6)); } } } } writeCharacteristic( UUID_SERVICE_PRIVATE, UUID_CHARACTERISTIC_PRIVATE1, command ); } break; case MotionEvent.ACTION_MOVE: if(mode==WALK) { switch(w_mode){ case FwdBack: //FWD, BACK pos[0] = points.get("" + event.getPointerId(0)); pos[0].x = event.getX(0); pos[0].y = event.getY(0); command[0] = 127; //FWD BACK if(pos[0].x > width/2)command[1] = 0; else command[1] = 1; if (pos[0].y < height * 0.2) { command[2] = 0; } else if (pos[0].y > height * 0.8) { command[2] = 100; } else { command[2] = (byte) (100 * (pos[0].y - height * 0.2) / (height * 0.6)); } break; case RightStep: for ( int i = 0; i < count; i++ ) { pos[i] = points.get("" + event.getPointerId(i)); pos[i].x = event.getX(i); pos[i].y = event.getY(i); points.put(""+pointerID, new PointF(event.getX(i), event.getY(i))); } command[0] = 126; if(!w_phase)command[1] = 0; else command[1] = 1; if (pos[1].x < width * 0.1) { command[2] = 100; } else if (pos[1].x > width * 0.4) { command[2] = 0; } else { command[2] = (byte) (100 * (width * 0.4 - pos[1].x) / (width * 0.3)); } break; case RightTurn: for ( int i = 0; i < count; i++ ) { pos[i] = points.get("" + event.getPointerId(i)); pos[i].x = event.getX(i); pos[i].y = event.getY(i); points.put(""+pointerID, new PointF(event.getX(i), event.getY(i))); } command[0] = 125; if(!w_phase)command[1] = 0; else command[1] = 1; if (pos[1].y < height * 0.2) { command[2] = 0; } else if (pos[1].y > height * 0.8) { command[2] = 100; } else { command[2] = (byte) (100 * (pos[1].y - height * 0.2) / (height * 0.6)); } break; case LeftStep: for ( int i = 0; i < count; i++ ) { pos[i] = points.get("" + event.getPointerId(i)); pos[i].x = event.getX(i); pos[i].y = event.getY(i); points.put(""+pointerID, new PointF(event.getX(i), event.getY(i))); } command[0] = 124; if(!w_phase)command[1] = 0; else command[1] = 1; if (pos[1].x < width * 0.6) { command[2] = 0; } else if (pos[1].x > width * 0.9) { command[2] = 100; } else { command[2] = (byte) (100 * (pos[1].x - width * 0.6) / (width * 0.3)); } break; case LeftTurn: for ( int i = 0; i < count; i++ ) { pos[i] = points.get("" + event.getPointerId(i)); pos[i].x = event.getX(i); pos[i].y = event.getY(i); points.put(""+pointerID, new PointF(event.getX(i), event.getY(i))); } command[0] = 123; if(!w_phase)command[1] = 0; else command[1] = 1; if (pos[1].y < height * 0.2) { command[2] = 0; } else if (pos[1].y > height * 0.8) { command[2] = 100; } else { command[2] = (byte) (100 * (pos[1].y - height * 0.2) / (height * 0.6)); } break; } writeCharacteristic( UUID_SERVICE_PRIVATE, UUID_CHARACTERISTIC_PRIVATE1, command ); try { Thread.sleep(100); } catch (InterruptedException e) { } }else if(mode==ARM) { command[0] = 122; command[1] = command[3] = 50; command[2] = command[4] = 50; for (int i = 0; i < count; i++) { pos[i] = points.get("" + event.getPointerId(i)); pos[i].x = event.getX(i); pos[i].y = event.getY(i); points.put("" + pointerID, new PointF(event.getX(i), event.getY(i))); } for (int i = 0; i < count; i++) { if (i == 0) { if (pos[0].x > width / 2) { if (pos[0].x < width * 0.6) { command[1] = 0; } else if (pos[0].x > width * 0.9) { command[1] = 90; } else { command[1] = (byte) (90 * (pos[0].x - width * 0.6) / (width * 0.3)); } if (pos[0].y < height * 0.2) { command[2] = 90; } else if (pos[0].y > height * 0.8) { command[2] = 0; } else { command[2] = (byte) (90 - 90 * (pos[0].y - height * 0.2) / (height * 0.6)); } } else { if (pos[0].x < width * 0.1) { command[3] = 90; } else if (pos[0].x > width * 0.4) { command[3] = 0; } else { command[3] = (byte) (90 * (width * 0.4 - pos[0].x) / (width * 0.3)); } if (pos[0].y < height * 0.2) { command[4] = 90; } else if (pos[0].y > height * 0.8) { command[4] = 0; } else { command[4] = (byte) (90 - 90 * (pos[0].y - height * 0.2) / (height * 0.6)); } } } if (i == 1) { if (pos[1].x > width / 2) { if (pos[1].x < width * 0.6) { command[1] = 0; } else if (pos[1].x > width * 0.9) { command[1] = 90; } else { command[1] = (byte) (90 * (pos[1].x - width * 0.6) / (width * 0.3)); } if (pos[1].y < height * 0.2) { command[2] = 90; } else if (pos[1].y > height * 0.8) { command[2] = 0; } else { command[2] = (byte) (90 - 90 * (pos[1].y - height * 0.2) / (height * 0.6)); } } else { if (pos[1].x < width * 0.1) { command[3] = 90; } else if (pos[1].x > width * 0.4) { command[3] = 0; } else { command[3] = (byte) (90 * (width * 0.4 - pos[1].x) / (width * 0.3)); } if (pos[1].y < height * 0.2) { command[4] = 90; } else if (pos[1].y > height * 0.8) { command[4] = 0; } else { command[4] = (byte) (90 - 90 * (pos[1].y - height * 0.2) / (height * 0.6)); } } } } writeCharacteristic(UUID_SERVICE_PRIVATE, UUID_CHARACTERISTIC_PRIVATE1, command); try { Thread.sleep(100); } catch (InterruptedException e) { } } break; case MotionEvent.ACTION_UP: break; case MotionEvent.ACTION_CANCEL: break; } return true; } private void connect() { if( mDeviceAddress.equals( "" ) ) { return; } if( null != mBluetoothGatt ) { return; } BluetoothDevice device = mBluetoothAdapter.getRemoteDevice( mDeviceAddress ); mBluetoothGatt = device.connectGatt( this, false, mGattcallback ); } private void disconnect() { if( null == mBluetoothGatt ) { return; } mBluetoothGatt.close(); mBluetoothGatt = null; mButton_Connect.setEnabled( true ); mButton_Disconnect.setEnabled( false ); mButton_LTRN.setEnabled( false ); mButton_FWRD.setEnabled( false ); mButton_RTRN.setEnabled( false ); mButton_LEFT.setEnabled( false ); mButton_STOP.setEnabled( false ); mButton_RGHT.setEnabled( false ); mButton_LED_ON.setEnabled( false ); mButton_BWRD.setEnabled( false ); mButton_LED_OFF.setEnabled( false ); mButton_SND.setEnabled( false ); } private void writeCharacteristic( UUID uuid_service, UUID uuid_characteristic, byte[] data ) { if( null == mBluetoothGatt ) { return; } BluetoothGattCharacteristic blechar = mBluetoothGatt.getService( uuid_service ).getCharacteristic( uuid_characteristic ); blechar.setValue(data); mBluetoothGatt.writeCharacteristic( blechar ); } }
ImageView上をドラッグするとonTouchイベントが呼ばれます。指を動かすとACTION_MOVEのところへ行き座標が取得できます。座標が領域の右側か左側か、縦方向の位置を0-100の値に変換してコマンドとして送信します。コマンドはcommand[0]:WALKモードであることの識別値(127)、command[1]:右か左か(右:1 左:0)、command[2]:縦方向位置(0-100)となっていて、ESP32側で歩行プログラムの配列から関節角度を計算します。
<= マイクロマシーン チュートリアル(13)
チュートリアルベース
マイクロマシーン チュートリアル (17) =>
Pingback: ロボットキット チュートリアル インデックス | RoboCreators | Meuse Robotics
Pingback: ロボットキット チュートリアル インデックス | cog | Meuse Robotics