Hi,
Currently the code is having the wheels move in a circle and the sweep motion is not really going. The pixy isn't really controlling anything like it's supposed to. What should I change to have the pixy chase the object and then sweep it plus grab it? This is what I have so far. There is a base of four servos and two sweep servos.
#include "SPI.h"
#include <Servo.h>
#include <Pixy2.h>
#include <PIDLoop.h>
#define MAX_TRANSLATE_VELOCITY 250
PIDLoop panLoop(350, 0, 600, true);
PIDLoop tiltLoop(500, 0, 700, true);
PIDLoop rotateLoop(300, 600, 300, false);
PIDLoop translateLoop(400, 800, 300, false);
Servo ServoBaseBRPin;
Servo ServoBaseBLPin;
Servo ServoBaseFRPin;
Servo ServoBaseFLPin;
Servo ServograbarmL;
Servo ServograbarmR;
int pos = 0;
Pixy2 pixy;
int buzzerpin=12;
//BR means back right pin FR front right pin
void setup() {
Serial.begin(115200);
ServoBaseBRPin.attach(2);
ServoBaseBLPin.attach(3);
ServoBaseFRPin.attach(4);
ServoBaseFLPin.attach(5);
ServograbarmL.attach(8);
ServograbarmR.attach(9);
ServoBaseBRPin.write(0);
ServoBaseBRPin.write(0);
ServoBaseFLPin.write(0);
ServoBaseFRPin.write(0);
ServograbarmL.write(0);
ServograbarmR.write(0);
pixy.init();
pixy.changeProg("color_connected_components");
// put your setup code here, to run once:
}
int16_t acquireBlock()
{
if (pixy.ccc.numBlocks && pixy.ccc.blocks[0].m_age>30)
return pixy.ccc.blocks[0].m_index;
return -1;
}
Block trackBlock(uint8_t index)
{
uint8_t i;
for (i=0; i<pixy.ccc.numBlocks; i++)
{
if (index==pixy.ccc.blocks.m_index)*
_ return &pixy.ccc.blocks*;_
_ }_
_ return NULL;_
_}_
void loop()
_{_
static int16_t index = -1;
int32_t panOffset, tiltOffset, headingOffset, left, right;
Block *block=NULL;
pixy.ccc.getBlocks();
if (index>-0)
_{_
_ block = trackBlock(index);_
if (block)
_{_
panOffset = (int32_t)pixy.frameWidth/2 - (int32_t)block->m_x;
tiltOffset = (int32_t)block->m_y - (int32_t)pixy.frameHeight/2;
_ panLoop.update(panOffset);_
_ tiltLoop.update(tiltOffset);_
pixy.setServos(panLoop.m_command, tiltLoop.m_command);
_ ServoBaseBLPin.write(0);_
ServoBaseBRPin.write(90);
ServoBaseFLPin.write(0);
ServoBaseFRPin.write(90);
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);
_}_
else
_{_
_ rotateLoop.reset();_
_ translateLoop.reset();_
_ rotateLoop.reset();_
_ translateLoop.reset();_
_ ServoBaseBLPin.write(0);_
_ ServoBaseBRPin.write(0);_
_ ServoBaseFLPin.write(0);_
_ ServoBaseFRPin.write(0);_
_ index = -1;_
panOffset = (int32_t)pixy.frameWidth/2-(int32_t)pixy.ccc.blocks[0].m_x;
tiltOffset - (int32_t)pixy.ccc.blocks[0].m_y-(int32_t)pixy.frameHeight/2;
_ panLoop.update(panOffset);_
_ tiltLoop.update(tiltOffset);_
pixy.setServos(panLoop.m_command,tiltLoop.m_command);
_}_
_}_
else
_{_
_ panLoop.reset();_
_ tiltLoop.reset();_
pixy.setServos(panLoop.m_command, tiltLoop.m_command);
_}_
_}*_
Thank you for the help