买了个pixy,才知道原来网上资源那么少,而且开发者也好像不接着开发了,真是伤心。
以下代码是arduino里面写的,用两个舵机搭了一个云台,然后pixy摄像头设别一个颜色的物体,然后跟着那个物体动的程序。
#include <SPI.h>
#include <Pixy.h>
#include <Servo.h> //库函数
#define P 0.02 //比例
#define D 0.05 //微分
#define A 10 //定位精度
int X_CENTER=160; //定义屏幕中点
int Y_CENTER=100;
int panError, tiltError,pre_panError, pre_tiltError,dpanError,dtiltError; //水平和垂直误差
int j;
Servo myservo1; //定义舵机
Servo myservo2;
Pixy pixy; //调用pixy
int pre_x=90,pre_y=90; //定义舵机初始角度
void printall() //输出各个参数
{
Serial.print("x ");
Serial.print("y ");
Serial.print("panError ");
Serial.print("tiltError ");
Serial.print("pre_x ");
Serial.print("pre_y ");
Serial.print("Error ");
Serial.println(" ");
Serial.print(pixy.blocks[0].x);
Serial.print(" ");
Serial.print(pixy.blocks[0].y);
Serial.print(" ");
Serial.print(panError);
Serial.print(" ");
Serial.print(tiltError);
Serial.print(" ");
Serial.print(pre_x);
Serial.print(" ");
Serial.println(pre_y);
}
void setup()
{
myservo1.attach(3);
myservo2.attach(5);
Serial.begin(9600);
Serial.print("Starting...\n"); //初始设定
}
void loop()
{
static int i = 0;
uint16_t blocks;
char buf[32];
blocks = pixy.getBlocks(); //物体数量
if(blocks) //有读取到物体就触发
{
panError=X_CENTER-pixy.blocks[0].x;
tiltError=Y_CENTER-pixy.blocks[0].y;
dpanError=panError-pre_panError;
dtiltError=tiltError-pre_tiltError;
if(pre_x>=0&&pre_x<=180&&pre_y>=0&&pre_y<=180) //防止超调
{
//Serial.println("a");
if(panError>=A||panError<=-A||tiltError>=A||tiltError<=-A) //定位精度
{
//偏差量赋值
pre_x=pre_x+P*panError+D*dpanError;
pre_y=pre_y+P*tiltError+D*dtiltError;
for(i=0;i<1;i++) //对误差值做出的动作
{
myservo1.write(pre_x);
myservo2.write(pre_y);
}
pre_panError=panError;
pre_tiltError=tiltError;
}
}
else
{
pre_x=90;
pre_y=90;
}
}
j=j+1;
if(j%10000==0)
{
printall();
}
}
|