2023电赛E题代码

openmv：

import sensor, image, time
from pyb import LED, UART
#import lcd
import json, ustruct

class Elecdesign(object):

    def __init__(self):
        sensor.reset()
        # 图像翻转
        # sensor.set_vflip(True)
        # sensor.set_hmirror(True)
        sensor.set_pixformat(sensor.RGB565)
        sensor.set_framesize(sensor.QVGA) # 320x240
        #sensor.set_windowing((480, 480))
        sensor.skip_frames(time=900)
        sensor.set_auto_gain(False)
        sensor.set_auto_whitebal(False)
        # 获取三种颜色的灯
        self.red_led = LED(1)
        self.green_led = LED(2)
        self.blue_led = LED(3)
        #self.red_threshold = (92, 0, -56, 34, -36, 76)
        self.red_threshold = (100, 0, -84, 17, -59, 94)
        #self.green_threshold = (100, 0, -39, 40, -67, 127)
        self.green_threshold = (100, 0, 93, -12, -54, 71)
        self.uart_middle = UART(3, 19200)
        #self.uart_middle.init(115200, bits=8, parity=None, stop=1)
        #self.uart_green = UART(1, 115200)
        # 误差消除
        self.square_detlax = 2
        self.square_detlay = 2
        #
        self.real_roi = (70, 48, 126, 143)  # 320 240
        self.left_corner = (70, 48)  # 左上角点
        self.left_down_corner = 0  # 左下角
        self.right_corner = (196, 191)  # 右下角点
        self.right_up_corner = 0  # 右上角
        self.w = 0
        self.h = 0
        self.middle_dot = 0    # 中心点

    # 关灯
    def close_rgb(self):
        self.red_led.off()
        self.green_led.off()
        self.blue_led.off()

    # 开白灯
    def while_rgb(self):
        self.close_rgb()
        self.red_led.on()
        self.green_led.on()
        self.blue_led.on()

    # 开红灯
    def red_rgb(self):
        self.close_rgb()
        self.red_led.on()

    # 开绿灯
    def green_rgb(self):
        self.close_rgb()
        self.green_led.on()

    # 开蓝灯
    def blue_rgb(self):
        self.close_rgb()
        self.blue_led.on()

    # 与arduino通信
    def chat_arduino(self, string):
        self.uart_middle.write(json.dumps(string))
        print("已发送：", string)

    # 接受消息
    def recv_arduino(self):
        recv = self.uart_middle.read()
        if recv:
            return recv
        else:
            return None

    # 返回指定色块相对于图像中的坐标（中心点）
    def dot_identify(self, thresholds, img):
        most_pixels = 0
        max_x = 0
        if img:
            #blob0 = img.find_blobs([thresholds], pixels_threshold=2, area_threshold=2, merge=True, invert=True)
            blob0 = img.find_blobs([thresholds],x_stride=1, y_stride=1, area_threshold=0, pixels_threshold=0,merge=False, margin=1, invert=True)
            if blob0:
                for n in range(len(blob0)):
                    if blob0[n].pixels() > most_pixels:
                        most_pixels = blob0[n].pixels()
                        max_x = n
                img.draw_rectangle(blob0[max_x].x(), blob0[max_x].y(), blob0[max_x].w(), blob0[max_x].h(),
                                        color=(0, 0, 255), thickness=1, fill=False)
                img.draw_cross(blob0[max_x].x() + int(blob0[max_x].w() / 2), blob0[max_x].y() + int(
                                    blob0[max_x].h() / 2), color=(0, 0, 255), size=1, thickness=1)
                return blob0[max_x].x() + int(blob0[max_x].w() / 2), \
                       blob0[max_x].y() + int(blob0[max_x].h() / 2)
            else:
                return 0
        else:
            return 0

    def change_mode_dot(self):
        sensor.reset()
        sensor.set_auto_gain(False)
        sensor.set_pixformat(sensor.RGB565)
        sensor.set_framesize(sensor.QVGA)  # 320x240
        sensor.skip_frames(n=10)
        sensor.set_auto_exposure(False, 1000)#在这里调节曝光度，调节完可以比较清晰地看清激光点
        sensor.set_auto_whitebal(False) # turn this off.
        sensor.set_auto_gain(False) # 关闭增益（色块识别时必须要关）


    def change_mode_square(self):
        sensor.reset()
        sensor.set_pixformat(sensor.RGB565)
        #sensor.set_framesize(sensor.QQVGA)  # 240x160
        sensor.set_framesize(sensor.QVGA)  # 320x240
        sensor.skip_frames(n=10)
        sensor.set_auto_whitebal(False)
        sensor.set_auto_gain(False)

    def find_max_square(self, blobs):
        max_size=0
        for blob in blobs:
            if blob[2]*blob[3] > max_size:
                max_blob=blob
                max_size = blob[2]*blob[3]
        return max_blob

    def square_type_change(self, data):
        string = ""
        for j in data:
            string = string + str(j)
        return "{"+string+"}"

    # 找方块
    def square_identify(self, img):
        blobs = img.find_rects(roi=self.real_roi, threshold = 30000)
        if blobs:
            r = self.find_max_square(blobs)
        else:
            return 0
        img.draw_rectangle(r.rect(), color = (255, 0, 0))
        #print(r.corners())
        dot1 = (r.corners()[0][0]+self.square_detlax, r.corners()[0][1]-self.square_detlay)
        dot2 = (r.corners()[1][0]-self.square_detlax, r.corners()[1][1]-self.square_detlay)
        dot3 = (r.corners()[2][0]-self.square_detlax, r.corners()[2][1]+self.square_detlay)
        dot4 = (r.corners()[3][0]+self.square_detlax, r.corners()[3][1]+self.square_detlay)
        corners = (dot1, dot4, dot3, dot2)
        for p in corners:
            img.draw_circle(p[0], p[1], 1, color = (0, 255, 0))
        #print(self.square_type_change(corners))
        #return self.square_type_change(corners)
        #print(corners)
        return corners

    # 像素点到具体坐标转换（中间为原点）
    def change_dot_type(self, dot):
        if dot == 0:
            return 0
        x, y = dot
        x0, y0 = self.left_corner
        xt, yt = self.right_corner
        return int((((x-x0)*0.5)/(xt-x0)-0.25)*1000), int((-((y-y0)*0.5)/(yt-y0)+0.25)*1000)

    # 左上角到右下角点推roi
    def count_roi(self):
        if self.left_corner and self.right_corner:
            self.real_roi = (self.left_corner[0], self.left_corner[1],\
            self.right_corner[0]-self.left_corner[0], self.right_corner[1]-self.left_corner[1])
            return True
        else:
            return False


    # 初始化幕布大小
    #def init_background(self):
        #type_dot = 0  # 1 左上 2 右下
        #while 1:
            #while 1:
                #data = self.recv_arduino()
                #if data == "left":
                    #type_dot = 1
                    #break
                #elif data == "right":
                    #type_dot = 2
                    #break
                #elif data == "EOF":
                    #return
            #sensor.skip_frames(n=10)
            #img = sensor.snapshot()
            #if type_dot == 1:
                #self.left_corner = self.dot_identify(self.red_threshold, img)
            #elif type_dot == 2:
                #self.right_corner = self.dot_identify(self.red_threshold, img)

    def run_dot(self):
        self.change_mode_dot()
        while 1:
            img = sensor.snapshot()
            print(self.dot_identify(self.red_threshold, img))

    def run_square(self):
        self.change_mode_square()
        while 1:
            img = sensor.snapshot().lens_corr(strength = 1.8, zoom = 1.0)
            self.square_identify(img)
            #lcd.display(img)

    def run_chat(self):
        while 1:
            data = self.recv_arduino()
            if data:
                data = data.decode("utf-8")
            time.sleep(1)

    # 计算其他两个点
    def find_two_dot(self):
        self.left_down_corner = (self.right_corner[0], self.left_corner[1])
        self.right_up_corner = (self.left_corner[0], self.right_corner[1])
        self.middle_dot = (int((self.left_corner[0] + self.right_corner[0])/2), \
        int((self.left_corner[1] + self.right_corner[1])/2))
        self.h = self.right_corner[1] - self.left_corner[1]
        self.w = self.right_corner[0] - self.left_corner[0]
        self.count_roi()

    # 初始化
    def first_init(self):
        #self.change_mode_dot()
        while 1:
            data = self.recv_arduino()
            sensor.snapshot()
            if data:
                data = data.decode("utf-8")
                print("接受消息：", data)
                if data=="O":
                    break
        while 1:
            img = sensor.snapshot()
            dot = self.dot_identify(self.red_threshold, img)
            if dot:
                print("dot1", dot)
                self.left_corner = dot
                break
        while 1:
            data = self.recv_arduino()
            sensor.snapshot()
            if data:
                data = data.decode("utf-8")
                print("接受消息：", data)
                if data=="O":
                    break
        while 1:
            img = sensor.snapshot()
            dot = self.dot_identify(self.red_threshold, img)
            if dot:
                print("dot2", dot)
                self.right_corner = dot
                break
        self.find_two_dot()
        print("初始化成功，四个点为：", self.left_corner, self.right_up_corner, self.right_corner, self.left_down_corner)
        self.green_rgb()
        time.sleep(1)
        self.close_rgb()

    # 发送边框
    def send_four_dot(self):
        dot_real_set = []
        while 1:
            img = sensor.snapshot().lens_corr(strength = 1.8, zoom = 1.0)
            dot_set = self.square_identify(img)
            if dot_set:
                for dot in dot_set:
                    dot_real_set.append(self.change_dot_type(dot))
                #return self.square_type_change(tuple(dot_real_set))
                dot_real_set = self.square_type_change(tuple(dot_real_set))
                self.uart_middle.write(str(dot_real_set))
                print("已发送：", str(dot_real_set))
                self.green_rgb()
                time.sleep(1)
                self.close_rgb()
                break

    # 发送单点
    def send_one_dot(self):
        while 1:
            sensor.snapshot()
            data = self.recv_arduino()
            #print(data)
            if data:
                data = data.decode("utf-8")
                print(data)
                if data == "D":
                    return
                if data == "O":
                    img = sensor.snapshot()
                    dot = self.dot_identify(self.red_threshold, img)
                    print(dot)
                    self.uart_middle.write("{"+str(self.change_dot_type(dot))+"}")
                    #print(str(self.change_dot_type(dot)))

    def main_run(self):
        while 1:
            order = self.recv_arduino()
            sensor.snapshot()
            if order:
                print(order)
                order = order.decode("utf-8")
                print("接受消息：", order)
                if order=="A":
                    #self.recv_arduino()
                    self.red_rgb()
                    self.change_mode_dot()
                    self.first_init()
                elif order=="B":
                    self.blue_rgb()
                    self.change_mode_square()
                    self.send_four_dot()
                elif order=="C":
                    self.change_mode_dot()
                    self.send_one_dot()

    def test_run1(self):
        strr = "{(-379, 3)(290, 38)(258, 430)(-411, 409)}"
        #self.change_mode_square()
        while 1:
            a = self.send_four_dot()
            #order = self.recv_arduino()
            #if order:
                #self.uart_middle.write(strr)
                #print("已发送：", strr)

    def print_dot(self):
        #new_list = []
        f = open("text.txt", "w")
        self.change_mode_dot()
        while 1:
            order = self.recv_arduino()
            sensor.snapshot()
            if order:
                order = order.decode("utf-8")
                if order == "O":
                    img = sensor.snapshot()
                    dot = self.dot_identify(self.red_threshold, img)
                    print(dot)
                    f.write(str(dot)+"\n")
                    #f.flush()
                elif order == "E":
                    f.close()
                    break


if __name__ == '__main__':
    recognition = Elecdesign()
    #recognition.run_dot()
    #recognition.run_square()
    recognition.main_run()
    #recognition.test_run1()
    #recognition.print_dot()

arduino（主控）:

#include <LobotServoController.h>
#include <SoftwareSerial.h>   // 引用BT函数库
#include <math.h>
#include <U8g2lib.h>
#include <OneButton.h>

#define KEY_FUNCTION 7  // 功能键
#define KEY_STOP 5      // 急停按键
#define light_ring 13

OneButton button_7(KEY_FUNCTION, false);
OneButton button_5(KEY_STOP, false);
U8G2_SSD1306_128X64_NONAME_1_SW_I2C u8g2(U8G2_R0, /* clock=*/ SCL, /* data=*/ SDA, /* reset=*/ U8X8_PIN_NONE); 
LobotServoController myse;
SoftwareSerial BT(8, 9); // 接收脚，传送脚
LobotServo  servos[2];

uint8_t type=1;  // 模式 1 reset 2 绕边框移动 3 寻黑边
int stop_sign=0;
int positionxxx;
int id1=1;
int id2=2;
int init_positionx = 1500;
int init_positiony = 1515;
int edge_positionx;
int edge_positiony;
int positionx1;
int positiony1;
int time=100;
int i;
double xangle;//横轴角度
double yangle;//纵轴角度
double x;//横轴坐标
double y;//纵轴坐标
double deltax=0.01;//横轴偏移
double deltay=0;
//double deltay=(7*PI)/250;//纵轴调整  
//double deltay=56/(PI*2000);
// double deltay=56/2000*PI;
double r=0.06;//舵机长度，舵机选择半径
double psi;//计算角度
double k_x=4000/(3*PI);//横轴为270°舵机
double k_y=2000/PI;  //纵轴为180°舵机
double result1;
double result2;

// 创建接受结构体
typedef struct
{
  int data[4][2] = {{0,0}};
  int len = 0;
}List;

typedef struct
{
  int data[1][2] = {{0,0}};
}Dot;

typedef struct
{ 
  int target_val;     //目标值
  int actual_val;     //实际值
  float err;            //定义当前偏差值
  float err_next;       //定义下一个偏差值
  float err_inter;  
  float err_last;       //定义最后一个偏差值
  float Kp, Ki, Kd;     //定义比例、积分、微分系数
}_pid;

_pid turnx;
_pid turny;

String detectString();
int chuanhan(float x, float y, float r);
Dot recv_one_dot();
List recv_four_dot();
void alert();
void stop_alert();
void click_7();
void longclick_7();
void click_6();
void click_5();
void surround();
void surround_black();
void UI_show_flash();
void UI_show_reset();
void UI_show_stop();
void UI_show_rect();
void UI_show_edge();
void UI_show_error();

void setup() {
  Serial.begin(9600); //舵机控制板控制串口
  //Serial.println("BT is ready!");
  BT.begin(19200);   //openmv串口
  u8g2.begin();  // 启动u8g2 128×64
  u8g2.setFont(u8g2_font_callite24_tr);
  servos[0].ID=id1;
  servos[1].ID=id2;
  pinMode(light_ring, OUTPUT);  // 声光提示
  button_7.reset();//清除一下按钮状态机的状态
  button_7.attachClick(click_7);  // 单击
  button_7.attachLongPressStart(longclick_7);  // 长按
  button_5.reset();
  button_5.attachClick(click_5);
}

void turn_param_init(void)
{
  turnx.target_val=0;
  turnx.actual_val=0.0;
  turnx.err = 0.0;
  turnx.err_last = 0.0;
  turnx.err_inter = 0.0;

  turnx.Kp = 1.0;
  turnx.Ki = 0.1;
//  turn.Kd = 1;
}

void turn_param_initY(void)
{
  turny.target_val=0;
  turny.actual_val=0.0;
  turny.err = 0.0;
  turny.err_last = 0.0;
  turny.err_next = 0.0;
 
  turny.Kp = 2;
  turny.Ki = 0.1;
//  turn.Kd = 1;
}

float turn_pid_realizeX(float actual_val)
{
  /*计算目标值与实际值的误差*/
  turnx.err = -turnx.target_val + actual_val;
  Serial.print("err:");
  Serial.println(turnx.err);
  if(turnx.err<10 && turnx.err>-10)
    {
      turnx.target_val=0;
      turnx.actual_val=0.0;
      turnx.err = 0.0;
      turnx.err_last = 0.0;
      turnx.err_inter = 0.0;
      
      // goto EXIT;
    }
  /*PID算法实现*/
//  turnx.actual_val = turnx.actual_val+ (0*(turnx.err - turnx.err_next) + 1*turnx.err);
//                   + turn.Kd*(turn.err - 2 * turn.err_next + turn.err_last);
  else{
    turnx.actual_val += 0.3*(turnx.err) + 0.2*turnx.err_inter;
//                   + turn.Kd*(turn.err - 2 * turn.err_next + turn.err_last);
//  /*传递误差*/
//  turn.err_last = turn.err_next;
    turnx.err_inter += turnx.err;
    Serial.print("actual_val:");
    Serial.println(turnx.err_inter);
    Serial.println(turnx.actual_val);}
//  turn.err_last = turn.err_next;
//  turnx.err_next = turnx.err;
  /*返回当前实际值*/
  return turnx.actual_val;
}

float turn_pid_realizeY(float actual_val)
{
  /*计算目标值与实际值的误差*/
  turny.err = turny.target_val-actual_val;
  /*PID算法实现*/
  turny.actual_val += turny.Kp*(turny.err - turny.err_next) + turny.Ki*turny.err;
//                   + turn.Kd*(turn.err - 2 * turn.err_next + turn.err_last);
  /*传递误差*/
//  turn.err_last = turn.err_next;
  turny.err_next = turny.err;
  /*返回当前实际值*/
  return turny.actual_val;
}

String detectString() // 判断传入的字符串能否被接收
{
  // 我们传入的数据形式 you send: {(90, 101)}
  while(BT.read() != '{');

  //返回"{}"中的字符串
  return(BT.readStringUntil('}'));
}

double x_chuanhan(double x)
{
  // x=-x;
  // xangle=atan(x);
  // result1=k_x*xangle+1500;
  // return result1;

  psi=atan(1/(deltax+x));
  if(x>0){xangle=acos(deltax/(sqrt(1+sq(deltax+x))))-psi;}
  if(x<0){xangle=PI-(acos(deltax/(sqrt(1+sq(deltax+x))))-psi);}
  if(x<0){result1=-x*x+k_x*xangle+1500;}
  if(x>0){result1=-x*x-k_x*xangle+1500;}
  return result1;

  // result1=-428.68*x+1499;
  // return result1;
}

double y_chuanhan(double x, double y)
{
  if (r+y>0){
    psi=atan(sqrt(1+sq(x))/(r+y));
    yangle=acos(r/(sqrt(1+sq(x)+sq(r+y))))-psi-deltay;
    result2=k_y*(0.850882)*yangle+1515;}
  else if (r+y<0){
    psi=atan(sqrt(1+sq(x))/(r+y));
    yangle=acos(-(r/(sqrt(1+sq(x)+sq(r+y)))))+psi+deltay;
    result2=-k_y*yangle+1515;}
  else if (r+y==0){
    yangle=asin(-r);
    result2=k_y*yangle+1515;}
  return result2;

  // result2=-192*sq(y)-572*y+1515;
  // return result2;
}


Dot recv_one_dot(){
  Dot dot;
  // BT.print("1");
  // Serial.println("123");
  while(1){
    if(BT.available()) break;
  }
  // Serial.println("321");
  String s = detectString();
  // Serial.println(s);
  String numStr = "";
  for(int i = 0; i<s.length(); i++)
  {
    if(s[i]=='(')
    {
      numStr = "";
    }
    else if(s[i] == ',')
    {
      dot.data[0][0] = numStr.toInt();
      
      numStr = "";  // 清空numstr，方便接受','后的另一组数据
    }
    else if(s[i]==')') // 读到结尾
    {
      dot.data[0][1] = numStr.toInt();
      numStr = "";
    }
    else  // 读到数据
    {
      numStr += s[i];
    }
  }
  return dot;  // 拿到一个数据点 有放大1000倍
}

List recv_four_dot(){
  List four_dot;
  while(1){
    if(BT.available()) break;
  }
  delay(500);  // 等待数据发完
  String s = detectString();
  Serial.println(s);
  String numStr = "";
  for(int i = 0; i<s.length(); i++)
  {
    if(s[i]=='(')
    {
      numStr = "";
    }
    else if(s[i] == ',')
    {
      four_dot.data[four_dot.len][0] = numStr.toInt();
      
      numStr = "";  // 清空numstr，方便接受','后的另一组数据
    }
    else if(s[i]==')') // 读到结尾
    {
      four_dot.data[four_dot.len][1] = numStr.toInt();
      numStr = "";
      four_dot.len++;
    }
    else  // 读到数据
    {
      numStr += s[i];
    }
  }
  // Serial.println(four_dot.data[0][0]);
  // Serial.println(four_dot.data[0][1]);
  // Serial.println(four_dot.data[1][0]);
  // Serial.println(four_dot.data[1][1]);
  // Serial.println(four_dot.data[2][0]);
  // Serial.println(four_dot.data[2][1]);
  // Serial.println(four_dot.data[3][0]);
  // Serial.println(four_dot.data[3][1]);
  return four_dot;  // 四个点，按顺序读取即可
}

void alert(){
  digitalWrite(light_ring, HIGH);
}

void stop_alert(){
  digitalWrite(light_ring, LOW);
}

void UI_show_flash(){
  char m_str[1];
  strcpy(m_str, u8x8_u8toa(type, 1));
  u8g2.firstPage();
  do {
    u8g2.drawStr(0, 40,"Mode:");
    u8g2.drawStr(70, 40, m_str);//在指定位置显示字符串
  } while ( u8g2.nextPage() );
}

void UI_show_reset(){
  u8g2.firstPage();
  do {
    u8g2.drawStr(0, 40,"reset!");
  } while ( u8g2.nextPage() );
}

void UI_show_stop(){
  u8g2.firstPage();
  do {
    u8g2.drawStr(0, 40,"stop!");
  } while ( u8g2.nextPage());
}

void UI_show_edge(){
  u8g2.firstPage();
  do {
    u8g2.drawStr(0, 40,"Edge!");
  } while ( u8g2.nextPage());
}

void UI_show_rect(){
  u8g2.firstPage();
  do {
    u8g2.drawStr(0, 40,"Rect!");
  } while ( u8g2.nextPage());
}

void UI_show_rect_1(){
  u8g2.firstPage();
  do {
    u8g2.drawStr(0, 40,"Rect_1!");
  } while ( u8g2.nextPage());
}

void UI_show_error(){
  u8g2.firstPage();
  do {
    u8g2.drawStr(0, 40,"error!");
  } while ( u8g2.nextPage());
}

void click_7(){
  Serial.println("click");
  type++;
  if(type>5){
    type=1;
  }
  UI_show_flash();
}

void longclick_7(){
  // Serial.println("long_click");
  if(type==1){  // reset  
    UI_show_reset();
    click_6();
  }else if(type==2){
    UI_show_edge();  // 绕边框移动
    surround();
  }else if(type==3){
    UI_show_rect_1();
    rect_1();
  }else if(type==4){
    UI_show_rect();   // 绕黑边移动
    surround_black();
    
  }else if(type==5){
    // PID_turn();
    tick();
  }
  else{
    UI_show_error();
  }
}

void tick(){
  for(int i=0;i<=215;i+=2)
{
  servos[0].Position=1605-i;
  for(int j=0;j<=286;j+=2)
{
  servos[1].Position=1646-j;
  myse.moveServos(servos,2,100);
  delay(110);
  BT.print("O");
  delay(200);
 }
}
  BT.print("E");
}

// void PID_turn(){
//    while(1)
//   {
//   if (turn.err>4 || turn.err<-4)
//   {
//     BT.print("O");
//     dot=recv_one_dot();
//     turn.actual_val=dot.data[0];
//     positionx=turn_pid_realize()+500;
//     servos[0].Position=positionx;
//     myse.moveServos(servos,2,200);
//   }
//   else
//   {
//     break;
//   }
//   }
// }

void PID_turn(Dot dot){
  //actual_val=dot.data[0][0];
  // if (turnx.err>4 || turnx.err<-4)
  // {
    // BT.print("O");
    // dot=recv_one_dot();
    // positionx=turn_pid_realize()+500;
    positionxxx=turn_pid_realizeX(dot.data[0][0])+1500;
    if(turnx.err!=0.0)
    {servos[0].Position=positionxxx;
    Serial.println("123");
    Serial.println(servos[0].Position);}
    // myse.moveServos(servos,2,200);
  // }
}

// 复位函数
void click_6(){
  // Serial.println("reset!");
  myse.moveServo(id1,init_positionx,50);
  myse.moveServo(id2,init_positiony,50);
  UI_show_reset();
  delay(1000);
}

// 具体写死的移动函数
void surround(){
  BT.print("A");
  delay(1000);
  // 左上
  servos[0].Position=1605;
  servos[1].Position=1646;
  myse.moveServos(servos,2,200);
  delay(220);
  delay(1000);
  BT.print("O");  // 告诉openmv快看
  delay(1000);
  // 右上
  servos[0].Position=1390;
  servos[1].Position=1646;
  myse.moveServos(servos,2,200);
  delay(220);
  delay(1000);
  // 右下
  servos[0].Position=1390;
  servos[1].Position=1360;
  myse.moveServos(servos,2,200);
  delay(220);
  delay(1000);
  BT.print("O");   // 告诉openmv快看
  delay(1000);
  // 左下
  servos[0].Position=1605;
  servos[1].Position=1360;
  myse.moveServos(servos,2,200);
  delay(220);
  delay(1000);
  // 左上
  servos[0].Position=1605;
  servos[1].Position=1646;
  myse.moveServos(servos,2,200);
  delay(220);
  delay(1000);
  // 回中
  servos[0].Position=init_positionx;
  servos[1].Position=init_positiony;
  myse.moveServos(servos,2,200);
  delay(220);
  UI_show_flash();
  return;
}

void rect_1(){
  delay(1000);
  // 左上
  servos[0].Position=1604;
  servos[1].Position=1643; 
  myse.moveServos(servos,2,200);
  delay(220);
  delay(2000);
  // 右上
  servos[0].Position=1486;
  servos[1].Position=1644; 
  myse.moveServos(servos,2,500);
  delay(520);
  delay(2000);
  // 右下
  servos[0].Position=1486;
  servos[1].Position=1537; 
  myse.moveServos(servos,2,200);
  delay(220);
  delay(2000);
  // 左下
  servos[0].Position=1604;
  servos[1].Position=1536; 
  myse.moveServos(servos,2,200);
  delay(220);
  delay(2000);
  // 左上
  servos[0].Position=1602;
  servos[1].Position=1639; 
  myse.moveServos(servos,2,200);
  delay(220);
  delay(2000);
  // 归中
  servos[0].Position=init_positionx;
  servos[1].Position=init_positiony; 
  myse.moveServos(servos,2,200);
  delay(220);

}

void surround_black(){
  BT.print("B");
  delay(1000);
  List dot_list = recv_four_dot();
  dot_list.len = 0;
  // BT.print("C");
  // delay(1000);
  for(dot_list.len; dot_list.len<=3; dot_list.len++){
// EXIT:
    // Serial.println(dot_list.data[dot_list.len][0]);
    // Serial.println(dot_list.data[dot_list.len][1]);
    // Serial.println(double(dot_list.data[dot_list.len][0]/1000.0), 3);
    // Serial.println(double(dot_list.data[dot_list.len][1]/1000.0), 3);
    servos[0].Position=x_chuanhan(double(dot_list.data[dot_list.len][0])/1000.00000);
    servos[1].Position=y_chuanhan(double(dot_list.data[dot_list.len][0])/1000.00000, double(dot_list.data[dot_list.len][1])/1000.00000);
    myse.moveServos(servos,2,200);
    delay(220);
    delay(1000);
    // turnx.target_val = dot_list.data[dot_list.len][0];
    // Serial.println("qweqwe");
    // Serial.println(turnx.target_val);
    // while(1){
    // // delay(1000);
    // BT.print("O");
    // Dot dot = recv_one_dot();
    // Serial.println("321");
    // Serial.println(dot.data[0][0]);
    // PID_turn(dot);
    // if(turnx.err<10 && turnx.err>-10)
    // { 
    //   turnx.err=0;
    //   break;
    // }
    // myse.moveServos(servos,2,200);
    // delay(220);
    // delay(1000);
    // }
  }
    servos[0].Position=x_chuanhan(double(dot_list.data[0][0])/1000.00000);
    servos[1].Position=y_chuanhan(double(dot_list.data[0][0])/1000.00000, double(dot_list.data[0][1])/1000.00000);
    myse.moveServos(servos,2,200);
    delay(220);
    delay(1000);
  // BT.print("D");
}

void click_5(){
  stop_sign=1;
}

void loop(){
  button_7.tick();
}