openMv的代码

import sensor, image, time
import json
from pid import PID
from pyb import Servo
from pyb import UART
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(10)
sensor.set_auto_whitebal(False)
clock = time.clock()
uart = UART(1, 9600)
green_threshold   = (98, 42, -17, 2, -34, -14)
size_threshold = 2000
x_pid = PID(p=0.5, i=1, imax=100)
h_pid = PID(p=0.05, i=0.1, imax=50)
def find_max(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
while(True):
	clock.tick()
	img = sensor.snapshot()
	blobs = img.find_blobs([green_threshold])
	if blobs:
		max_blob = find_max(blobs)
		x_error = max_blob[5]-img.width()/2
		h_error = max_blob[2]*max_blob[3]-size_threshold
		print("x error: ", x_error)
		img.draw_rectangle(max_blob[0:4])
		img.draw_cross(max_blob[5], max_blob[6])
		x_output=x_pid.get_pid(x_error,1)
		h_output=h_pid.get_pid(h_error,1)
		pcx=-h_output-x_output
		pcy=-h_output+x_output
		data={
		"cx":pcx,
		"cy":pcy}
		data_out = json.dumps(data)
		uart.write(data_out +'\n')
	else:
		data={
		"cx":50,
		"cy":-50}
		data_out = json.dumps(data)
		uart.write(data_out +'\n')

arduino的代码

#include <Wire.h>
#include "QGPMaker_MotorShield.h"
#include <ArduinoJson.h>

QGPMaker_MotorShield AFMS = QGPMaker_MotorShield();
//QGPMaker_Servo *Servo4 = AFMS.getServo(3);
QGPMaker_DCMotor *DCMotor_2 = AFMS.getMotor(2);
QGPMaker_DCMotor *DCMotor_4 = AFMS.getMotor(4);
QGPMaker_DCMotor *DCMotor_1 = AFMS.getMotor(1);
QGPMaker_DCMotor *DCMotor_3 = AFMS.getMotor(3);

volatile int cx=0;
volatile int cy=0;
volatile char z=0;
String json="";

float leftspeed=0;
float rightspeed=0;

void setup(){
  AFMS.begin(50);
  Serial2.begin(9600);
  Serial.begin(9600);
}

void loop(){
  if (Serial2.available() > 0) 
   {
    z = char(Serial2.read());
    json = String(json) + String(z);
    if (z == '}')
     {
      DynamicJsonDocument doc(200); //声明一个JsonDocument对象
      deserializeJson(doc, json);
      JsonObject obj = doc.as<JsonObject>();
      float cx = doc["cx"];
      float cy = doc["cy"];
      if (cx!=0&&cy!=0)
       {
        leftspeed=cx;
        rightspeed=cy;
        Serial.print("cx = ");
        Serial.print(cx);
        Serial.print("cy = ");
        Serial.println(cy);
        }
        json = "";
      }
    }
   //run1(leftspeed,rightspeed);
   if (leftspeed < 0){
    DCMotor_1->run(BACKWARD);
    DCMotor_2->run(BACKWARD);
  }else{
    DCMotor_1->run(FORWARD);
    DCMotor_2->run(FORWARD);}
    DCMotor_1->setSpeed(int(abs(leftspeed)));
    DCMotor_2->setSpeed(int(abs(leftspeed)));
  if (rightspeed < 0){
    DCMotor_3->run(BACKWARD);
    DCMotor_4->run(BACKWARD);
}else{
    DCMotor_3->run(FORWARD);
    DCMotor_4->run(FORWARD);
}
    DCMotor_3->setSpeed(int(abs(rightspeed)));
    DCMotor_4->setSpeed(int(abs(rightspeed)));
}