買了個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();
}
}
|
