M5StickCとM5StickVを連動させ、RoverCを動かす

M5StickCとVを連動させて、Vの方で画像を認識し、それを追従するRoverを作りたいのですが、うまくStickCとVの連動ができません。何かプログラムでおかしなところや忘れがきな設定等あれば教えていただきたいです。

下記サイトを参考に制作しています
RoverCとM5StickCとM5StickVでグリッパロボットを作る
[RoverC_TrackBall]
(https://github.com/icyqwq/RoverC_TrackBall)
M5StickC + RoverC + UnitV でオブジェクトを追跡させる方法

C
1
2#include <M5StickC.h>
3#include <math.h>
4#include<Wire.h>
5#include<VL53L0X.h>
6
7HardwareSerial VSerial(1);
8TFT_eSprite tft = TFT_eSprite(&M5.Lcd);
9
10uint8_t I2CWrite1Byte(uint8_t Addr, uint8_t Data)
11{
12  Wire.beginTransmission(0x38);
13  Wire.write(Addr);
14  Wire.write(Data);
15  return Wire.endTransmission();
16}
17
18uint8_t I2CWritebuff(uint8_t Addr, uint8_t *Data, uint16_t Length)
19{
20  Wire.beginTransmission(0x38);
21  Wire.write(Addr);
22  for (int i = 0; i < Length; i++)
23  {
24    Wire.write(Data[i]);
25  }
26  return Wire.endTransmission();
27}
28
29uint8_t Setspeed(int16_t Vtx, int16_t Vty, int16_t Wt)
30{
31  int16_t speed_buff[4] = {0};
32  int8_t speed_sendbuff[4] = {0};
33
34  Wt = (Wt > 100) ? 100 : Wt;
35  Wt = (Wt < -100) ? -100 : Wt;
36
37  Vtx = (Vtx > 100) ? 100 : Vtx;
38  Vtx = (Vtx < -100) ? -100 : Vtx;
39  Vty = (Vty > 100) ? 100 : Vty;
40  Vty = (Vty < -100) ? -100 : Vty;
41
42  Vtx = (Wt != 0) ? Vtx * (100 - abs(Wt)) / 100 : Vtx;
43  Vty = (Wt != 0) ? Vty * (100 - abs(Wt)) / 100 : Vty;
44
45  speed_buff[0] = Vty - Vtx - Wt;
46  speed_buff[1] = Vty + Vtx + Wt;
47  speed_buff[3] = Vty - Vtx + Wt;
48  speed_buff[2] = Vty + Vtx - Wt;
49
50  for (int i = 0; i < 4; i++)
51  {
52    speed_buff[i] = (speed_buff[i] > 100) ? 100 : speed_buff[i];
53    speed_buff[i] = (speed_buff[i] < -100) ? -100 : speed_buff[i];
54    speed_sendbuff[i] = speed_buff[i];
55  }
56  return I2CWritebuff(0x00, (uint8_t *)speed_sendbuff, 4);
57}
58
59void setup() {
60  Serial.begin(115200);
61  M5.begin();
62  M5.Lcd.setRotation(0);
63  M5.Lcd.fillScreen(0);
64
65  tft.setColorDepth(8);
66  tft.createSprite(80, 160);
67  tft.setTextDatum(TC_DATUM);
68  tft.setTextColor(WHITE);
69  tft.setTextSize(2);
70
71  VSerial.begin(115200, SERIAL_8N1, 33, 32);     // Vと接続
72  Wire.begin(0, 26);
73
74  Setspeed(0, 0, 0);
75
76  VSerial.write(0xAF);
77  tft.fillSprite(TFT_ORANGE);
78  tft.pushSprite(0, 0);
79
80  
81}
82
83int16_t ux;
84unsigned long T;
85#define BASE_SPEED  20
86bool last_dir = false;
87void loop()
88{
89  M5.update();
90  if (M5.BtnA.wasReleased())
91  {
92    Setspeed(0, 0, 0);
93    ESP.restart();
94  }
95/*
96 * VとCを接続
97*/
98  if (VSerial.available())
99  {
100    VSerial.write(0xAF);
101
102    uint8_t b_data[4];               //4か2かどっちか
103    VSerial.readBytes(b_data, 4);
104
105    int8_t ux = b_data[0];
106
107    uint32_t area = b_data[1] << 16 | b_data[2] << 8 | b_data[3];
108    int8_t uy;
109    Serial.printf("%d, %d\n", ux, area);
110    if (area < 500)
111    {
112      if (last_dir)
113        Setspeed(0, 0, 15);
114      else
115        Setspeed(0, 0, -15);
116      tft.fillSprite(TFT_RED);
117      tft.pushSprite(0, 0);
118      return;
119    }
120    else
121    {
122      tft.fillSprite(TFT_GREEN);
123      tft.pushSprite(0, 0);
124      if (area < 20000)
125      {
126        uy = 1 / (0.00001 * area) + 15;
127        if (uy > 40)
128          uy = 40;
129        if (uy < 5)
130          uy = 5;
131        Setspeed(ux, uy, 0);
132      }
133      else if (area > 24000)
134      {
135        Setspeed(ux, -10, 0);
136      }
137      else
138      {
139        Setspeed(ux, 0, 0);
140      }
141    }
142
143    if (ux > 0)
144    {
145      last_dir = true;
146    }
147    else
148    {
149      last_dir = false;
150    }
151  }
152}

Python
1
2import sensor
3import image
4import lcd
5import time
6import utime
7from machine import UART
8from Maix import GPIO
9from fpioa_manager import *
10
11Px = -0.4
12
13fm.register(34,fm.fpioa.UART1_TX)
14fm.register(35,fm.fpioa.UART1_RX)
15uart_out = UART(UART.UART1, 115200, 8, None, 1, timeout=1000, read_buf_len=4096)
16
17sensor.reset()
18sensor.set_pixformat(sensor.RGB565)
19sensor.set_framesize(sensor.QVGA)
20sensor.set_hmirror(0)             #カメラ向き左右反転Vの取付位置がさかさだから
21sensor.set_auto_gain(0)           #検出安定化のためオートゲインオフ
22sensor.set_auto_whitebal(0)       #オートホワイトオフ
23sensor.run(1)
24
25#target_lab_threshold = (12, 71, 30, 70, 2, 50)   #R
26target_lab_threshold = (35, 80, -45, -15, -3, 40)   #G
27prev_area = 0
28
29while False:
30    uart_out.write('???\n')
31    utime.sleep_ms(100)
32
33while True:
34    img = sensor.snapshot()
35
36    blobs = img.find_blobs([target_lab_threshold], x_stride = 2, y_stride = 2, pixels_threshold = 100, merge = True, margin = 20)
37    if blobs:
38      max_area = 0
39      target = blobs[0]
40        for b in blobs:
41          if b.area() > max_area:
42            max_area = b.area()
43            target = b
44              if uart_out.read(4096):
45                 area = target.area()
46                 dx = 160 - target[5]                #UnitVとM5stackVではカメラ取付向きが違うためｘセンター位置を320pix/2,ｘ軸パラメータをtarget[6]→[5]へ変更
47                 hexlist = [(dx >> 8) & 0xFF, dx & 0xFF, (area >> 16) & 0xFF, (area >> 8) & 0xFF, area & 0xFF]
48                 uart_out.write(bytes(hexlist))
49                    else:
50                        pass
51                      print(target.area())
52                      tmp=img.draw_rectangle(target[0:4])
53                      tmp=img.draw_cross(target[5], target[6])
54                      c=img.get_pixel(target[5], target[6])
55
56                    else:
57                       if uart_out.read(4096):
58                          hexlist = [0x80, 0x00, 0x00, 0x00, 0x00]
59                          uart_out.write(bytes(hexlist))
60                    else:
61                            pass
62
63#        xerr = 160 - target[5]
64 #       ux = int(Px * xerr)
65
66
67
68
69
70'''
71       if ux > 100:
72            ux = 100
73        elif ux < -100:
74            ux = -100
75
76        if abs(xerr) < 20:
77            ux = 0
78        #print(ux)
79
80'''
81        if uart_out.read(4096):
82            area = target.area()
83            hexlist = [ux & 0xFF, (area >> 16) & 0xFF, (area >> 8) & 0xFF, area & 0xFF]
84            a = uart_out.write(bytes(hexlist))
85'''
86        #print(abs(prev_area - target.area()))
87        prev_area = target.area()
88        tmp=img.draw_rectangle(target[0:4])
89        tmp=img.draw_cross(target[5], target[6])
90        # c=img.get_pixel(target[5], target[6])
91    else:
92        if uart_out.read(1):
93            hexlist = [0x00, 0x00, 0x00, 0x00]
94            a = uart_out.write(bytes(hexlist))
95'''
96