Hello All,
I’m trying to do something that should be pretty easy. I’ve made a code that works similar to the pan/tilt demo, but moves a gimbal that the pixy isn’t attached to. This way I can have a laser track a target without having the bulky pixy board directly attached. I’ve done this by converting the x & y coordinates of the blocks to angles for the two servos. Now I’m trying to make the laser return to the center after it loses the target. I’ve attached my current code below. When I comment out the else statement at the bottom of the code, it tracks perfectly but when it’s not commented and trying to return it to center after 1 second of losing the target, it looks to the far right and shakes when it detects blocks. How can I get it to return to center after 1 second? Thanks in advance.
#include <Servo.h>
Servo yaxis;
Servo xaxis;
#include <SPI.h>
#include <Pixy.h>
int xpos = 0;
int ypos = 0;
// This is the main Pixy object
Pixy pixy;
unsigned long rust;
void setup()
{
Serial.begin(9600);
Serial.print(“Starting…\n”);
yaxis.attach (9);
xaxis.attach (10);
pixy.init();
}
void loop()
{
static int i = 0;
int j;
uint16_t blocks;
char buf[32];
int x= ((pixy.blocks[j].x*.5625)-90); // scale pixels to +/- 90 degrees
int y= (pixy.blocks[j].y);
// grab blocks!
blocks = pixy.getBlocks();
// If there are detect blocks, print them!
if (blocks)
{
i++;
// do this (print) every 5 frames because printing every
// frame would bog down the Arduino
if (i%5==0)
{
sprintf(buf, "Detected %d:\n", blocks);
Serial.print(buf);
for (j=0; j<blocks; j++)
{
sprintf(buf, " block %d: ", j);
Serial.print(buf);
pixy.blocks[j].print(); // prints x and y coordinates of blocks
xpos=(x-160)*41.25/100+140; // change servo position x variable
xaxis.write (xpos); //move servo to x position
// Serial.println(xpos);
ypos=-(y-100)*26.25/100+85; // change servo position y variable
yaxis.write (ypos); //move servo to y position
Serial.println(ypos);
}
}
}
else {
rust = millis();
if (rust = 1000)
{
xaxis.write (140);
yaxis.write (85);
Serial.println (rust);
}
}
}