Hi there!
I’m very new and still a beginner in robotics, coding etc… Now my problem. I built me an own robot, with a Pixy2, a PanTilt module, Arduino Mega 2560 and instead of a zumo shield I used an Arduino MotorShield where I connected 2 motors at the A and B ports. Vin and GND is connected with a battery case with 5x AA batteries. I want to use the example script “ccc_zumo_chase” to run my little robot. I also tried to change the script, in hope that it works with my Arduino Shield. But it doesn’t work, the robot detects the object and also the PanTilt module follows it… But the robot only drives forward and also the speed stays the same. Can someone help me to get it to work?
That was my try:
//
// begin license header
//
// This file is part of Pixy CMUcam5 or "Pixy" for short
//
// All Pixy source code is provided under the terms of the
// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
// Those wishing to use Pixy source code, software and/or
// technologies under different licensing terms should contact us at
// [email protected]. Such licensing terms are available for
// all portions of the Pixy codebase presented here.
//
// end license header
//
#include <Pixy2.h>
#include <PIDLoop.h>
// #include <ZumoMotors.h>
//Motor-Definitionen
int pinMotorChanA = 12;
int pinMotorChanB = 13;
int pinBrakeChanA = 9;
int pinBrakeChanB = 8;
int pinSpeedChanA = 3;
int pinSpeedChanB = 11;
#define MAX_TRANSLATE_VELOCITY 150
Pixy2 pixy;
PIDLoop panLoop(350, 0, 600, true);
PIDLoop tiltLoop(500, 0, 700, true);
PIDLoop rotateLoop(300, 600, 300, false);
PIDLoop translateLoop(400, 800, 300, false);
void setup()
{
Serial.begin(115200);
Serial.print("Starting...\n");
pinMode(pinMotorChanA, OUTPUT);
pinMode(pinMotorChanB, OUTPUT);
pinMode(pinBrakeChanA, OUTPUT);
pinMode(pinBrakeChanB, OUTPUT);
pinMode(pinSpeedChanA, OUTPUT);
pinMode(pinSpeedChanB, OUTPUT);
/* initialize motor objects
motors.setLeftSpeed(0);
motors.setRightSpeed(0); */
// need to initialize pixy object
pixy.init();
// user color connected components program
pixy.changeProg("color_connected_components");
}
// Take the biggest block (blocks[0]) that's been around for at least 30 frames (1/2 second)
// and return its index, otherwise return -1
int16_t acquireBlock()
{
if (pixy.ccc.numBlocks && pixy.ccc.blocks[0].m_age>30)
return pixy.ccc.blocks[0].m_index;
return -1;
}
// Find the block with the given index. In other words, find the same object in the current
// frame -- not the biggest object, but he object we've locked onto in acquireBlock()
// If it's not in the current frame, return NULL
Block *trackBlock(uint8_t index)
{
uint8_t i;
for (i=0; i<pixy.ccc.numBlocks; i++)
{
if (index==pixy.ccc.blocks[i].m_index)
return &pixy.ccc.blocks[i];
}
return NULL;
}
void loop()
{
static int16_t index = -1;
int32_t panOffset, tiltOffset, headingOffset, left, right;
Block *block=NULL;
pixy.ccc.getBlocks();
if (index==-1) // search....
{
Serial.println("Searching for block...");
index = acquireBlock();
if (index>=0)
Serial.println("Found block!");
}
// If we've found a block, find it, track it
if (index>=0)
block = trackBlock(index);
// If we're able to track it, move motors
if (block)
{
// calculate pan and tilt errors
panOffset = (int32_t)pixy.frameWidth/2 - (int32_t)block->m_x;
tiltOffset = (int32_t)block->m_y - (int32_t)pixy.frameHeight/2;
// calculate how to move pan and tilt servos
panLoop.update(panOffset);
tiltLoop.update(tiltOffset);
// move servos
pixy.setServos(panLoop.m_command, tiltLoop.m_command);
// calculate translate and rotate errors
panOffset += panLoop.m_command - PIXY_RCS_CENTER_POS;
tiltOffset += tiltLoop.m_command - PIXY_RCS_CENTER_POS - PIXY_RCS_CENTER_POS/2 + PIXY_RCS_CENTER_POS/8;
rotateLoop.update(panOffset);
translateLoop.update(-tiltOffset);
// keep translation velocity below maximum
if (translateLoop.m_command>MAX_TRANSLATE_VELOCITY)
translateLoop.m_command = MAX_TRANSLATE_VELOCITY;
// calculate left and right wheel velocities based on rotation and translation velocities
left = -rotateLoop.m_command + translateLoop.m_command;
right = rotateLoop.m_command + translateLoop.m_command;
// set wheel velocities
digitalWrite(pinMotorChanA,HIGH);
digitalWrite(pinBrakeChanA,LOW);
digitalWrite(pinSpeedChanA,(left));
digitalWrite(pinMotorChanB,HIGH);
digitalWrite(pinBrakeChanB,LOW);
digitalWrite(pinSpeedChanB,(right));
// print the block we're tracking -- wait until end of loop to reduce latency
block->print();
}
else // no object detected, stop motors, go into search state
{
rotateLoop.reset();
translateLoop.reset();
digitalWrite(pinBrakeChanA,HIGH);
digitalWrite(pinSpeedChanA,0);
digitalWrite(pinBrakeChanB,HIGH);
digitalWrite(pinSpeedChanB,0);
// motors.setLeftSpeed(0);
//motors.setRightSpeed(0);
index = -1; // set search state
}
}