今年のMakerFaireTokyo2020にて展示させていただいた「M5StickC+UnitV+toioでオートサッカー」について作り方を紹介します。
どんなものかはこちらの動画をご覧ください！
色付きサッカーボールを見せるとカメラで認識し、それをtoioでずっと追いかけます。
#toio でオートサッカー⚽️
— Akichika Tanaka (@akichika) October 4, 2020
AIカメラ #UnitV と #M5StickC そしてスイッチサイエンスさんより発売された「toio™️開発用プレイマット」を使って自動でサッカーするデモを作りました。今日 #MFTokyo2020 でtoio友の会のブースで展示していただいてます！https://t.co/WUBoXI1D10 #toiotomo #M5Stack pic.twitter.com/D3Jr1OrZBL
小型のM5StickCとUnitVとtoioを組み合わせてローカルに閉じたシステムで手軽に運用できるので発展性がありそうです。
必要なもの
-
toio コア キューブ（以降キューブ）
- toio本体セットに付属、もしくは単体で購入も可能 ※SwichScienceで販売されています
- toio 開発用プレイマット（スイッチサイエンス）
- M5StickC
- M5 UnitV
- スマホスタンド、ミニ三脚など上から撮影できる固定器具
- 色付きサッカーボール（蛍光オレンジやピンクなど、テーブルサッカー用が扱いやすい）
- レゴブロックやGoロボの人形等（サッカーのキャラクター用）
- ボールが飛び出ないようにするための段ボールの壁
組み立て方
まずUnitV、M5StickCそれぞれにプログラムを書き込みます。プログラムについては後述します。
次にUnitV付属の4PケーブルでM5StickCと接続します。
電気的にはこれで終了です。はんだ付け不要、ほとんど何もしていませんので超簡単です。必要に応じてM5StickCにUSBで充電したり電源を確保します。
マットの上空からUnitVで撮影するために、スマホスタンドを使います。自由に伸縮できるタイプの方が調整が楽です。
なお、「toio 開発用プレイマット」にはサッカーフィールドの画像（Adobe Stockで素材購入）をCMYのみで印刷しました。印刷にはEPSONのA3プリンターEP-982A3を使いました。この機種には「カラーインクで黒を表現する」モードがあり、toio開発用プレイマットの特殊パターンに影響を与えずに印刷可能でした。
詳しくはこちらの記事もご参考ください。
アルゴリズム
プログラムの作成においてはこちらの記事を大いに参考にさせていただき、プログラムも流用させていただきました。素晴らしい記事をありがとうございました。
##ボール認識
お手軽に実現するため、ボールの認識についてはUnitVのクロマキー（特定の色を認識）を使います。そのためボールにはできるだけ蛍光色などの特徴的なものを利用する方が良いと思います。抽出した色の重心位置をターゲット位置としてM5StickCへ送信します。
コードはUnitVのMaixPyで書きました。ほとんど上で紹介したこちらの記事のクロマキーのコードを微修正した程度です。書き込み方法等参考にしてみてください。
ボールの色を認識するための調整もしっかり行う必要があります。上記同様こちらの記事が参考になります。
なお、座標空間の合わせこみや・位置合わせは本来は補正やアフィン変換も含めてしっかりキャリブレーションしなければならないのですが、今回は簡単のために撮影位置をマットの真上にしてプログラムに実機を合わせこむことにしました。シンプルですが、とりあえず成立したので良しとします。
長いのでプログラム（ソースコード）は最後に掲載します。
##ボール追尾アルゴリズム
UnitVからM5StickCに送られてきた重心位置にtoioの目標指定移動機能を使ってキューブを動かします。ある範囲を超えたら中央に戻ることで、端の方に来た時に壁に向かってボールを押し続けてスタックしてしまうことを避けるようにしました。
toioとの通信にはいつもお世話になっている@KatsuShun89さんのこちらの記事を参考にしました。本当にありがとうございます。
長いのでこちらもプログラム（ソースコード）は最後に掲載します。
あそび方
M5StickCの電源を入れると一番近くのキューブに接続します。ボールを見つけると自動で追いかけます。
サッカーと言っても、ゴールの認識や敵キャラまでは準備していないので、とりあえずドリブルし続けるだけとなっていますが、見た目には楽しいです。
マットの四隅を少し高くすると中央に向かってボールが転がってくるようになるので、キューブがボールを端までドリブルして中央に戻る間にボールが戻り、無限にドリブルし続けるような動きをするようになります。
最後にあらためて実機の動きを再掲します。
#toio でオートサッカー⚽️
— Akichika Tanaka (@akichika) October 4, 2020
AIカメラ #UnitV と #M5StickC そしてスイッチサイエンスさんより発売された「toio™️開発用プレイマット」を使って自動でサッカーするデモを作りました。今日 #MFTokyo2020 でtoio友の会のブースで展示していただいてます！https://t.co/WUBoXI1D10 #toiotomo #M5Stack pic.twitter.com/D3Jr1OrZBL
1人称バージョン
M5StickC+UnitV+toioを1つにまとめた「１人称視点」で追尾ものも作ってみました。
こちらの方がよりロボットらしくまとまってはいますが、M5StickCの電池が10分程度しか持たないのでデモは大変でした。
#toio とAIカメラ #UnitV & #M5StickC で実現した超小型完全自律サッカーロボット、つくばの #TMMF2020 会場では人間対ロボットで試合ができるようにしてお子さんたちに大人気です。かなり単純なアルゴリズムでもそこそこ動いてくれて何よりです。 pic.twitter.com/fD1bG6JGVz
— Akichika Tanaka (@akichika) February 15, 2020
#おわりに
toioとM5StickC、UnitVの組み合わせはサイズ感や手軽さ共にとてもマッチしているので、自律移動ロボット作りにもってこいです。ぜひみなさんも試してみてください！
#参考：プログラム一覧
##UnitV用のプログラム
参考記事
MaixPyで書き込みます。
boot.py
#original> https://qiita.com/airpocket/items/714aac02b132f486eb67
import sensor
import image
import lcd
import time
import utime
from machine import UART
from Maix import GPIO
from fpioa_manager import *
fm.register(34,fm.fpioa.UART1_TX)
fm.register(35,fm.fpioa.UART1_RX)
uart_out = UART(UART.UART1, 115200, 8, None, 1, timeout=1000, read_buf_len=4096)
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_hmirror(1) #カメラ左右反転
sensor.set_auto_gain(0) #検出安定化のためオートゲインオフ
sensor.set_auto_whitebal(0) #検出安定化のためオートホワイトバランスオフ
sensor.run(1)
while False:
uart_out.write('TEST\n')
utime.sleep_ms(100)
target_lab_threshold =(49, 100, -2, 73, 23, 103) #ボールに合わせて要調整
while True:
img=sensor.snapshot()
blobs = img.find_blobs([target_lab_threshold], x_stride = 2, y_stride = 2, pixels_threshold = 100, merge = True, margin = 20)
if blobs:
max_area = 0
target = blobs[0]
for b in blobs:
if b.area() > max_area:
max_area = b.area()
target = b
area = target.area()
dx = 160 - target[5] #UnitVとM5stackVではカメラ取付向きが違うためｘセンター位置を320pix/2,ｘ軸パラメータをtarget[6]→[5]へ変更
dy = 120 - target[6] #UnitVとM5stackVではカメラ取付向きが違うためｘセンター位置を320pix/2,ｘ軸パラメータをtarget[6]→[5]へ変更
hexlist = [(dx >> 8) & 0xFF, dx & 0xFF,(dy >> 8) & 0xFF, dy & 0xFF, (area >> 8) & 0xFF, area & 0xFF] #6Byte
print(hexlist, target[5], target[6], target.area())
if uart_out.read(4096):
uart_out.write(bytes(hexlist))
else:
pass
tmp=img.draw_rectangle(target[0:4])
tmp=img.draw_cross(target[5], target[6])
c=img.get_pixel(target[5], target[6])
else:
if uart_out.read(4096):
hexlist = [0x80, 0x00, 0x80, 0x00, 0x00, 0x00]
uart_out.write(bytes(hexlist))
else:
pass
##M5StickC用のプログラム
toioとM5Stack連携の参考記事
M5StickC用の設定をしたArduino SDKで書き込みます。
toio-soccer.ino
//original> https://qiita.com/KatsuShun89/items/1d3d1ccae226a5b73da8
#include <M5StickC.h>
#include <BLEDevice.h>
// toio service
static BLEUUID service_UUID("10B20100-5B3B-4571-9508-CF3EFCD7BBAE");
// read sensor characteristic
static BLEUUID read_char_UUID("10B20101-5B3B-4571-9508-CF3EFCD7BBAE");
// motor characteristic
static BLEUUID motor_char_UUID("10B20102-5B3B-4571-9508-CF3EFCD7BBAE");
// sound characteristic
static BLEUUID sound_char_UUID("10B20104-5B3B-4571-9508-CF3EFCD7BBAE");
static boolean do_connect = false;
static boolean connected = false;
static boolean do_scan = false;
static BLERemoteCharacteristic* read_characteristic;
static BLERemoteCharacteristic* motor_characteristic;
static BLERemoteCharacteristic* sound_characteristic;
static BLEAdvertisedDevice* my_device;
static bool is_btnA_switch = false;
static uint8_t motor_data_size = 8;
static uint8_t motor_pos_data_size = 13;
static uint8_t sound_data_size = 3;
static uint8_t current_sound = 0;
enum {
sound_enter = 0,
sound_selected,
sound_cancel,
sound_cursor,
sound_mat_in,
sound_mat_out,
sound_get_1,
sound_get_2,
sound_get_3,
sound_effect_1,
sound_effect_2,
sound_max, //threshold
};
typedef struct {
uint16_t x_cube_center;
uint16_t y_cube_center;
uint16_t angle_cube_center;
uint16_t x_read_sensor;
uint16_t y_read_sensor;
uint16_t angle_read_sensor;
} position_id_t;
typedef struct {
uint32_t standard_id;
uint16_t angle_cube;
} standard_id_t;
static position_id_t position_id = {0};
static standard_id_t standard_id = {0};
static bool is_missed_position_id = false;
static bool is_missed_standard_id = false;
static void readPositionID(uint8_t *data, size_t length)
{
memcpy(&position_id, &data[1], sizeof(position_id));
}
static void readStandardID(uint8_t *data, size_t length)
{
memcpy(&standard_id, &data[1], sizeof(standard_id));
}
static void readPositionIDMissed(uint8_t *data, size_t length)
{
is_missed_position_id = true;
position_id.x_cube_center = 0;
position_id.y_cube_center = 0;
position_id.angle_cube_center = 0;
position_id.x_read_sensor = 0;
position_id.y_read_sensor = 0;
position_id.angle_read_sensor = 0;
}
static void readStandardIDMissed(uint8_t *data, size_t length)
{
is_missed_standard_id = true;
standard_id.standard_id = 0;
standard_id.angle_cube = 0;
}
void (*readFunctionTable[])(uint8_t *data, size_t length) = {
&readPositionID,
&readStandardID,
&readPositionIDMissed,
&readStandardIDMissed
};
static void selectReadFunction(uint8_t *data, size_t length)
{
uint8_t id = data[0];
Serial.printf("read id:%d\n", id);
//need to size check
readFunctionTable[id - 1](data, length);
}
static void notifyReadCallback(BLERemoteCharacteristic* ble_remote_characteristic,
uint8_t* data,
size_t length,
bool is_notify) {
Serial.print("Notify callback for characteristic ");
Serial.print(ble_remote_characteristic->getUUID().toString().c_str());
Serial.print(" of data length ");
Serial.println(length);
Serial.print("data: ");
for (uint8_t i = 0; i < length; i++) {
Serial.printf("%x ", data[i]);
}
Serial.println("");
selectReadFunction(data, length);
}
class MyClientCallback : public BLEClientCallbacks {
void onConnect(BLEClient* client) {
}
void onDisconnect(BLEClient* client) {
connected = false;
Serial.println("onDisconnect");
}
};
bool connectToServer() {
Serial.print("Forming a connection to ");
Serial.println(my_device->getAddress().toString().c_str());
BLEClient* client = BLEDevice::createClient();
Serial.println(" - Created client");
client->setClientCallbacks(new MyClientCallback());
// Connect to the remove BLE Server.
client->connect(my_device); // if you pass BLEadvertised_device instead of address, it will be recognized type of peer device address (public or private)
Serial.println(" - Connected to server");
// Obtain a reference to the service we are after in the remote BLE server.
BLERemoteService* remote_service = client->getService(service_UUID);
if (remote_service == nullptr) {
Serial.print("Failed to find our service UUID: ");
Serial.println(service_UUID.toString().c_str());
client->disconnect();
return false;
}
Serial.println(" - Found toio service");
//read
read_characteristic = remote_service->getCharacteristic(read_char_UUID);
if (read_characteristic == nullptr) {
Serial.print("Failed to find read characteristic UUID: ");
Serial.println(read_char_UUID.toString().c_str());
client->disconnect();
return false;
}
Serial.println(" - Found read characteristic");
// Read the value of the characteristic.
if (read_characteristic->canRead()) {
std::string value = read_characteristic->readValue();
Serial.print("read characteristic value was: ");
Serial.println(value.c_str());
}
if (read_characteristic->canNotify())
read_characteristic->registerForNotify(notifyReadCallback);
//motor
motor_characteristic = remote_service->getCharacteristic(motor_char_UUID);
if (motor_characteristic == nullptr) {
Serial.print("Failed to find motor characteristic UUID: ");
Serial.println(motor_char_UUID.toString().c_str());
client->disconnect();
return false;
}
Serial.println(" - Found motor characteristic");
//sound
sound_characteristic = remote_service->getCharacteristic(sound_char_UUID);
if (sound_characteristic == nullptr) {
Serial.print("Failed to find sound characteristic UUID: ");
Serial.println(sound_char_UUID.toString().c_str());
client->disconnect();
return false;
}
Serial.println(" - Found sound characteristic");
connected = true;
}
/**
Scan for BLE servers and find the first one that advertises the service we are looking for.
*/
class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
/**
Called for each advertising BLE server.
*/
void onResult(BLEAdvertisedDevice advertised_device) {
Serial.print("BLE Advertised Device found: ");
Serial.println(advertised_device.toString().c_str());
// We have found a device, let us now see if it contains the service we are looking for.
if (advertised_device.haveServiceUUID() && advertised_device.isAdvertisingService(service_UUID)) {
BLEDevice::getScan()->stop();
my_device = new BLEAdvertisedDevice(advertised_device);
do_connect = true;
do_scan = true;
} // Found our server
} // onResult
}; // Myadvertised_deviceCallbacks
//-----Begin UnitV handler-----
HardwareSerial VSerial(1);
typedef struct
{
int16_t dx;
int16_t dy;
uint32_t area;
} v_response_t;
uint8_t I2CWrite1Byte(uint8_t Addr, uint8_t Data)
{
Wire.beginTransmission(0x38);
Wire.write(Addr);
Wire.write(Data);
return Wire.endTransmission();
}
uint8_t I2CWritebuff(uint8_t Addr, uint8_t *Data, uint16_t Length)
{
Wire.beginTransmission(0x38);
Wire.write(Addr);
for (int i = 0; i < Length; i++)
{
Wire.write(Data[i]);
}
return Wire.endTransmission();
}
uint8_t Setspeed(int16_t Vtx, int16_t Vty, int16_t Wt)
{
int16_t speed_buff[4] = {0};
int8_t speed_sendbuff[4] = {0};
Wt = (Wt > 100) ? 100 : Wt;
Wt = (Wt < -100) ? -100 : Wt;
Vtx = (Vtx > 100) ? 100 : Vtx;
Vtx = (Vtx < -100) ? -100 : Vtx;
Vty = (Vty > 100) ? 100 : Vty;
Vty = (Vty < -100) ? -100 : Vty;
Vtx = (Wt != 0) ? Vtx * (100 - abs(Wt)) / 100 : Vtx;
Vty = (Wt != 0) ? Vty * (100 - abs(Wt)) / 100 : Vty;
speed_buff[0] = Vty - Vtx - Wt;
speed_buff[1] = Vty + Vtx + Wt;
speed_buff[3] = Vty - Vtx + Wt;
speed_buff[2] = Vty + Vtx - Wt;
for (int i = 0; i < 4; i++)
{
speed_buff[i] = (speed_buff[i] > 100) ? 100 : speed_buff[i];
speed_buff[i] = (speed_buff[i] < -100) ? -100 : speed_buff[i];
speed_sendbuff[i] = speed_buff[i];
}
return I2CWritebuff(0x00, (uint8_t *)speed_sendbuff, 4); //NEED TO FIX FOR toio
}
enum
{
kNoTarget = 0,
kLeft,
kRight,
kStraight,
kTooClose
};
const uint16_t kThreshold = 30; // If target is in range ±kThreshold, the car will go straight
v_response_t v_data; // Data read back from V
uint8_t state = 0; // Car's movement status
//----End UnitV handler----///
void setupBLE() {
BLEDevice::init("");
// Retrieve a Scanner and set the callback we want to use to be informed when we
// have detected a new device. Specify that we want active scanning and start the
// scan to run for 5 seconds.
BLEScan* ble_scan = BLEDevice::getScan();
ble_scan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
ble_scan->setInterval(1349);
ble_scan->setWindow(449);
ble_scan->setActiveScan(true);
ble_scan->start(5, false);
}
void setup() {
M5.begin();
M5.Lcd.fillScreen(WHITE);
delay(100);
M5.Lcd.fillScreen(0x0000);
M5.Lcd.setTextFont(1);
M5.Lcd.setTextSize(1);
M5.Lcd.setTextColor(0xFFFF, 0x0000);
M5.Lcd.drawString("toio soccer", 5, 5);
//for UnitV
VSerial.begin(115200, SERIAL_8N1, 33, 32);
Wire.begin(0, 26);
//for toio
Serial.begin(115200);
Serial.println("Starting Arduino BLE Client application...");
setupBLE();
} // End of setup.
static String getSoundStr(void)
{
String sound_str;
switch (current_sound) {
case sound_enter:
sound_str = "Enter ";
break;
case sound_selected:
sound_str = "Selected";
break;
case sound_cancel:
sound_str = "Cancel ";
break;
case sound_cursor:
sound_str = "Cursor ";
break;
case sound_mat_in:
sound_str = "Mat in ";
break;
case sound_mat_out:
sound_str = "Mat out ";
break;
case sound_get_1:
sound_str = "Get 1 ";
break;
case sound_get_2:
sound_str = "Get 2 ";
break;
case sound_get_3:
sound_str = "Get 3 ";
break;
case sound_effect_1:
sound_str = "Effect 1";
break;
case sound_effect_2:
sound_str = "Effect 2";
break;
}
return sound_str;
}
int btnAhold = 0;
static void checkPushButton(void)
{
if (M5.BtnA.wasPressed()) {
setupBLE();
esp_restart();
}
if (M5.BtnB.wasPressed()) {
sendSoundControl();
current_sound++;
if (current_sound >= sound_max) current_sound = sound_enter;
}
M5.Lcd.drawString(getSoundStr(), 160, 180);
}
static void setMotorVelocity(uint8_t vel_l, uint8_t dir_l, uint8_t vel_r, uint8_t dir_r)
{
uint8_t data[motor_data_size] = {0};
if (is_btnA_switch == true)
{
vel_l = 0;
vel_r = 0;
}
else if (is_btnA_switch == false && (position_id.x_cube_center < 80 || position_id.x_cube_center > 410 || position_id.y_cube_center > 410 || position_id.y_cube_center < 80))
{
vel_l = 0;
vel_r = 0;
}
data[0] = 0x02; //motor control with specifiled time
data[1] = 0x01; //motor id 1 : left
data[2] = dir_l; //direction
data[3] = vel_l; //speed
data[4] = 0x02; //motor id 2 : right
data[5] = dir_r; //direction
data[6] = vel_r; //speed
data[7] = 0x0A; //control time
//Serial.println("write value");
motor_characteristic->writeValue(data, sizeof(data));
}
static void setPosition(uint16_t pos_x, uint16_t pos_y, uint16_t pos_dir, uint8_t vel)
{
uint8_t data[motor_pos_data_size] = {0};
data[0] = 0x03; //motor control with specifiled position
data[1] = 0x00; //
data[2] = 0x05; //time out
data[3] = 0x00; //control type
data[4] = vel; //velocity
data[5] = 0x00; //constant velocity
data[6] = 0x00; //RESERVED
data[7] = pos_x & 0xFF; //target position on X axis Low
data[8] = (pos_x >> 8) & 0xFF; //target position on X axis
data[9] = pos_y & 0xFF; //target position on Y axis LOW
data[10] = (pos_y >> 8) & 0xFF; //target position on Y axis
// data[11] = pos_dir & 0xFF; //target direction LOW
data[11] = 0x00;
// data[12] = (pos_dir >> 8) & 0xFF; //target direction
data[12] = 0xA0;
motor_characteristic->writeValue(data, sizeof(data));
}
static void sendSoundControl(void)
{
uint8_t data[sound_data_size] = {0};
data[0] = 0x02; //0x02:sound effect
data[1] = current_sound; //sound effects
data[2] = 0x10; //volume
//Serial.println("write value");
sound_characteristic->writeValue(data, sizeof(data));
}
static void drawReadSensor(void)
{
M5.Lcd.setTextSize(1);
String x_cube_str = String("X:") + String(position_id.x_cube_center) + String(" ");
M5.Lcd.drawString(x_cube_str, 10, 20);
String y_cube_str = String("Y:") + String(position_id.y_cube_center) + String(" ");
M5.Lcd.drawString(y_cube_str, 10, 30);
String angle_cube_str = String("Ang:") + String(position_id.angle_cube_center) + String(" ");
M5.Lcd.drawString(angle_cube_str, 10, 40);
String standard_id_str = String("StdID:") + String(standard_id.standard_id) + String(" ");
M5.Lcd.drawString(standard_id_str, 10, 50);
String standard_id_angle_str = String("StdAng:") + String(standard_id.angle_cube) + String(" ");
M5.Lcd.drawString(standard_id_angle_str, 10, 60);
if (is_btnA_switch == true) M5.Lcd.drawString("STOP", 10, 70);
else M5.Lcd.drawString("MOVE", 10, 70);
}
int counter = 0;
int flip = 0;
int wdt_counter = 0;
// This is the Arduino main loop function.
void loop() {
M5.update();
checkPushButton();
if (wdt_counter++ > 100) {
setupBLE();
esp_restart();
}
// If the flag "do_connect" is true then we have scanned for and found the desired
// BLE Server with which we wish to connect. Now we connect to it. Once we are
// connected we set the connected flag to be true.
if (do_connect == true) {
if (connectToServer()) {
Serial.println("We are now connected to the BLE Server.");
} else {
Serial.println("We have failed to connect to the server; there is nothin more we will do.");
}
do_connect = false;
}
// If we are connected to a peer BLE Server, update the characteristic each time we are reached
// with the current time since boot.
if (connected) {
drawReadSensor();
wdt_counter = 0;
} else if (do_scan) {
BLEDevice::getScan()->start(0); // this is just eample to start scan after disconnect, most likely there is better way to do it in arduino
}
//----Begin UnitV toio relation----
VSerial.write(0xAF);
if (VSerial.available())
{
uint8_t buffer[6]; //7
VSerial.readBytes(buffer, 6); //7
v_data.dx = (buffer[0] << 8) | buffer[1];
v_data.dy = (buffer[2] << 8) | buffer[3];
v_data.area = (buffer[4] << 8) | buffer[5];
Serial.printf("Buffer:%d %d %d %d %d %d ", buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]);
M5.Lcd.fillRect(0, 0, TFT_WIDTH, 79, BLACK);
M5.Lcd.fillRect(0, 80, TFT_WIDTH, TFT_HEIGHT, GREEN);
M5.Lcd.drawRect(10, 80, 60, 80, WHITE);
M5.Lcd.setTextSize(1);
String cx_str = String("cx:") + String(v_data.dx + 250);
String cy_str = String("cy:") + String(v_data.dy + 250);
M5.Lcd.drawString(cx_str, 40, 20);
M5.Lcd.drawString(cy_str, 40, 30);
if (position_id.x_cube_center > 90 && position_id.x_cube_center < 400 && position_id.y_cube_center > 140 && position_id.y_cube_center < 360) {
M5.Lcd.fillRect(40 - ((position_id.y_cube_center - 250) / 4), ((position_id.x_cube_center - 250) / 4) + 120, 8, 8, WHITE);
}
if (v_data.dx > -160 && v_data.dx < 160 && v_data.dy > -120 && v_data.dy < 120 && v_data.area > 100)
{
M5.Lcd.fillRect(((v_data.dy + 120) / 4) - 4 + 10, ((v_data.dx + 160) / 4) - 4 + 80, 8, 8, RED);
if (v_data.area > 200)
{
M5.Lcd.drawString("SHOOT", 10, 10);
if (counter == 0) {
if (v_data.dx > -140 && v_data.dx < 140 && v_data.dy > -100 && v_data.dy < 100)
setPosition((uint16_t)(v_data.dx + 250), (uint16_t)(-v_data.dy + 250), 0x00, 0x30);
else
setPosition((uint16_t)(250), (uint16_t)(250), 0x00, 0x30);
}
counter++;
if (counter > 50) {
counter = 0;
}
}
else
{
M5.Lcd.drawString("SEEK", 10, 10);
//setMotorVelocity(0x10,0x02,0x10,0x01);
counter = 0;
}
String area_str = String("a:") + String(v_data.area);
M5.Lcd.drawString(area_str, 35, 70);
}
else
{
M5.Lcd.drawString("LOST", 10, 10);
counter++;
if (counter > 20) {
counter = 0;
flip = (flip == 0) ? 1 : 0;
}
if (flip == 0)setMotorVelocity(0x8, 0x02, 0x08, 0x01);
else setMotorVelocity(0x08, 0x01, 0x08, 0x02);
M5.Lcd.drawString(String(flip), 40, 10);
M5.Lcd.drawString(String(counter), 60, 10);
}
M5.Lcd.setTextSize(1);
Serial.printf("%d, %d, %d, %d\n", v_data.dx, v_data.dy, v_data.area, state);
}
else
counter = 0;
//---- End UnitV toio relation ----
delay(20); // Delay a second between loops.
} // End of loop