Follow me
This function represent the real first robotic evolution of Ardway. I used a special camera called Pixy (http://www.cmucam.org/projects/cmucam5) (Pixy) The idea is to force ArduWay to follow a specific object identified through an initial configuration. The Pixy CmuCam is able to save in memory up to 7 objects (or colour patterns) and to send on a serial port all the positional informations. Those positions are evaluated and related to the camera view. While searching to put in the center of the camera the object (using the ArdWay Motors) the system moves and try to (constantly) “follow” the object.
Example of the follow-Me function
The following video describe how to manage the interaction with the FollowMe Kernel. The logic applied is to record a red object and to cycle on it continuously evaluating in the X/Y/Z are the different shift
The follow-Me Algorithm
WARNING: This alghorith is property of Vittorio Margherita. The author is not responsible for any damage caused by changes to the present software nor for the present software
installed on any hardware not properly controlled.
Be carefull before to connect the controller running this software to any mechnanical equipment. It could cause damage to persons or to things (this is not a game)
//This alghorith is property of Vittorio Margherita. The author is not responsible for any damage caused by changes to the present software nor to the present software
//installed on any hardware not properly controlled.
//Be carefull before to connect the controller running this software to any mechnanical equipment. It could cause damage to persons or to things (this is not a game)
if (SerialTransfer.isFLWM == true) {
TimeCounter++;
if (TimeCounter == 60) { //Every 6000 millisec check the pixy camera and verify what to do
//Check the status of every potentiometer to validate variables
CheckPotentiometer();
//If pixy doesn't find any object then start rotating searching for something
if (NotFound>100) {
if (NotFound > 65535)
NotFound = 101;
if ((NotFound == 101) || (NotFound == 102)) {
PanPosition = DEFAULT_PAN_CENTER;
TiltPosition = DEFAULT_TILT_CENTER;
Decrease = false;
FirstTime = true;
BackAndForward = true;
}
pixy.setServos(PanPosition, TiltPosition);
if (Decrease)
PanPosition -= 4;
else
PanPosition += 4;
if (PanPosition >= 1000)
Decrease = true;
if (PanPosition <= 0)
Decrease = false;
}
TimeCounter = 0;
//Get blocks to time optimize pixy performances (with a delay of 2 millisecs. Never go over 4)
blocks = pixy.getBlocks();
delay(4);
blocks = pixy.getBlocks();
if (blocks)
{
// do this (print) every 20 frames because printing every
// frame would bog down the Arduino
if (DEBUGON) {
sprintf(buf, "Detected %d:\n", blocks);
Serial.print(buf);
}
JMax = 99;
if (pixy.blocks[0].signature == 1) {
Prod = (pixy.blocks[0].width * pixy.blocks[0].height);
JMax = 0;
if (DEBUGON) {
sprintf(buf, "Found JMax in 0 with prod: %d\n", Prod);
Serial.print(buf);
}
}
if (JMax != 99) {
if (Prod > ObjectDimension) {
NotFound = 0;
sprintf(buf, "Width:%d, Height:%d", pixy.blocks[JMax].width, pixy.blocks[JMax].height);
tft.drawString(buf, 0, yDraw, 2);
yDraw = scroll_line();
if (DEBUGON) {
sprintf(buf, "Entering movement section with %d: ", JMax);
Serial.print(buf);
pixy.blocks[JMax].print();
}
//As first check if the object is going forward or Backward
if (FirstTime) {
pixy.setServos(DEFAULT_PAN_CENTER, DEFAULT_TILT_CENTER + 100);
FirstTime = false;
}
if (BackAndForward) {
if ((pixy.blocks[JMax].width - DimensionIncrement) > ObjectDistance) {
sprintf(buf, "Going BACKWARD with PEAK: %d", (pixy.blocks[JMax].width - ObjectDistance));
tft.drawString(buf, 0, yDraw, 2);
yDraw = scroll_line();
PeackValue = (pixy.blocks[JMax].width - ObjectDistance);
SerialTransfer.Direction = BACKWORD;
SerialTransfer.SendMotorPeak((pixy.blocks[JMax].width - ObjectDistance), 0);
for (i = 0; i < 25; i++) //This is the time needed for the serial communication to end. Empty the buffer
pixy.getBlocks();
}
else if ((pixy.blocks[JMax].width + DimensionIncrement) < ObjectDistance) {
sprintf(buf, "Going FORWARD with PEAK: %d", (ObjectDistance - pixy.blocks[JMax].width));
tft.drawString(buf, 0, yDraw, 2);
yDraw = scroll_line();
SerialTransfer.Direction = FORWARD;
SerialTransfer.SendMotorPeak((ObjectDistance - pixy.blocks[JMax].width), 0);
for (i = 0; i < 25; i++) //as before empty the buffer
pixy.getBlocks();
PeackValue = (ObjectDistance - pixy.blocks[JMax].width);
}
else
{
//Stop because the movment back and forward is too small
sprintf(buf, "Send a STOP object is not moving");
tft.drawString(buf, 0, yDraw, 2);
yDraw = scroll_line();
SerialTransfer.Direction = STOP;
PeackValue = 0;
SerialTransfer.SendMotorPeak(200, 0);
}
if (PeackValue > DimensionIncrement * 10) //NOTE: This is the real peack Value
BackAndForward = true;
else
BackAndForward = false;
}
else {
//Now check if the object is going left or right
if (pixy.blocks[JMax].x <= 160) {
Direction = RIGHT;
PeakTime = (160 - pixy.blocks[JMax].x) + PeakIncrement;
sprintf(buf, "Going LEFT with PEAK: %d", PeakTime);
}
else
{
Direction = LEFT;
PeakTime = (pixy.blocks[JMax].x - 159) + PeakIncrement;
sprintf(buf, "Going RIGHT with PEAK: %d", PeakTime);
}
//go for it (but move eliminating errors and spurious movements)
if (PeakTime > (PeakIncrement + PEAKBUFFER)) {
SerialTransfer.Direction = Direction;
tft.drawString(buf, 0, yDraw, 2);
yDraw = scroll_line();
SerialTransfer.SendMotorPeak(PeakTime, 0);
for (i = 0; i < 25; i++)
pixy.getBlocks();
}
else
{
//Stop because the movment Left and Right is too small
sprintf(buf, "Send a STOP object is not moving");
tft.drawString(buf, 0, yDraw, 2);
yDraw = scroll_line();
SerialTransfer.Direction = STOP;
PeackValue = 0;
SerialTransfer.SendMotorPeak(200, 0);
}
if (PeakTime > DimensionIncrement * 10 ) //NOTE: This is the real peack Value
BackAndForward = false;
else
BackAndForward = true;
}
}
else
NotFound++;
}
else
NotFound++;
}
else
NotFound++;
}
//delay(250);
}