Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Arduino 101 + Pixy
//
// begin license header
//
// This code is created by Ron Dagdag
//
// All 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
// ron@dagdag.net. Such licensing terms are available for
// all portions of the codebase presented here.
//
// end license header
//
// This sketch is a simple tracking that uses the pan/tilt unit for PixyCam, Also connects to Blynk. For
// more information, go here:
//
// https://www.hackster.io/RONDAGDAG/rocky-rover-bd16f0
//
#define BLYNK_PRINT Serial
#include <BlynkSimpleCurieBLE.h>
#include <CurieBLE.h>
#include <SPI.h>
#include <Pixy.h>
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"
// You should get Auth Token in the Blynk App.
// Go to the Project Settings (nut icon).
char auth[] = "f8d978f3a6de4a858ee7f8e37af882d5";
BLEPeripheral blePeripheral;
// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
// Select which 'port' M1, M2, M3 or M4.
Adafruit_DCMotor *motor1 = AFMS.getMotor(1);
Adafruit_DCMotor *motor2 = AFMS.getMotor(2);
Adafruit_DCMotor *motor3 = AFMS.getMotor(3);
Adafruit_DCMotor *motor4 = AFMS.getMotor(4);
Pixy pixy;
#define X_CENTER ((PIXY_MAX_X-PIXY_MIN_X)/2)
#define Y_CENTER ((PIXY_MAX_Y-PIXY_MIN_Y)/2)
#define RCS_MIN_POS 0L
#define RCS_MAX_POS 1000L
#define RCS_CENTER_POS ((RCS_MAX_POS-RCS_MIN_POS)/2)
bool running;
class ServoLoop
{
public:
ServoLoop(int32_t pgain, int32_t dgain);
void update(int32_t error);
void updatePan(int32_t error);
int32_t m_pos;
int32_t m_prevError;
int32_t m_pgain;
int32_t m_dgain;
};
ServoLoop panLoop(300, 500);
ServoLoop tiltLoop(500, 700);
ServoLoop::ServoLoop(int32_t pgain, int32_t dgain)
{
m_pos = PIXY_RCS_CENTER_POS;
m_pgain = pgain;
m_dgain = dgain;
m_prevError = 0x80000000L;
}
void ServoLoop::update(int32_t error)
{
long int vel;
char buf[32];
if (m_prevError!=0x80000000)
{
vel = (error*m_pgain + (error - m_prevError)*m_dgain)>>10;
//sprintf(buf, "%ld\n", vel);
//Serial.print(buf);
m_pos += vel;
if (m_pos>PIXY_RCS_MAX_POS)
{
m_pos = PIXY_RCS_MAX_POS;
}
else if (m_pos<PIXY_RCS_MIN_POS)
{
m_pos = PIXY_RCS_MIN_POS;
}
}
m_prevError = error;
}
void ServoLoop::updatePan(int32_t error)
{
long int vel;
char buf[32];
if (m_prevError!=0x80000000)
{
vel = (error*m_pgain + (error - m_prevError)*m_dgain)>>10;
sprintf(buf, "%ld\n", vel);
Serial.print(buf);
m_pos += vel;
if (m_pos>PIXY_RCS_MAX_POS)
{
m_pos = PIXY_RCS_MAX_POS;
motor1->run(FORWARD); //left
motor3->run(FORWARD);
running = true;
}
else if (m_pos<PIXY_RCS_MIN_POS)
{
m_pos = PIXY_RCS_MIN_POS; //right
motor2->run(FORWARD);
motor4->run(FORWARD);
running = true;
}
}
m_prevError = error;
}
void setup()
{
Serial.begin(9600);
Serial.print("Starting...\n");
// The name your bluetooth service will show up as, customize this if you have multiple devices
blePeripheral.setLocalName("RonD");
blePeripheral.setDeviceName("RonD");
blePeripheral.setAppearance(384);
pixy.init();
Blynk.begin(auth, blePeripheral);
blePeripheral.begin();
Serial.println("Waiting for connections...");
Serial.println("Adafruit Motorshield v2 - DC Motor");
AFMS.begin(); // create with the default frequency 1.6KHz
// Set the speed to start, from 0 (off) to 255 (max speed)
motor1->setSpeed(128);
motor2->setSpeed(128);
motor3->setSpeed(128);
motor4->setSpeed(128);
}
int32_t size = 400;
void loop()
{
Blynk.run();
blePeripheral.poll();
static int i = 0;
int j;
uint16_t blocks;
char buf[32];
int32_t panError, tiltError;
blocks = pixy.getBlocks();
if (blocks)
{
panError = X_CENTER-pixy.blocks[0].x;
tiltError = pixy.blocks[0].y-Y_CENTER;
panLoop.updatePan(panError);
tiltLoop.update(tiltError);
pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);
i++;
// do this (print) every 50 frames because printing every
// frame would bog down the Arduino
if (i%10==0)
{
int trackedBlock = 0;
sprintf(buf, "Detected %d:\n", blocks);
Serial.print(buf);
long maxSize = 0;
for (j=0; j<blocks; j++)
{
sprintf(buf, " block %d: ", j);
Serial.print(buf);
pixy.blocks[j].print();
/*Serial.println(pixy.blocks[j].width);
if (pixy.blocks[j].width > width)
{
width = pixy.blocks[j].width;
}*/
long newSize = pixy.blocks[j].height * pixy.blocks[j].width;
if (newSize > maxSize)
{
trackedBlock = j;
maxSize = newSize;
}
}
int32_t followError = RCS_CENTER_POS - panLoop.m_pos; // How far off-center are we looking now?
// Size is the area of the object.
// We keep a running average of the last 8.
size += pixy.blocks[trackedBlock].width * pixy.blocks[trackedBlock].height;
size -= size >> 3;
// Forward speed decreases as we approach the object (size is larger)
int forwardSpeed = constrain(400 - (size/256), -100, 400);
// Steering differential is proportional to the error times the forward speed
int32_t differential = (followError + (followError * forwardSpeed))>>8;
// Adjust the left and right speeds by the steering differential.
int leftSpeed = constrain(forwardSpeed + differential, -400, 400);
int rightSpeed = constrain(forwardSpeed - differential, -400, 400);
motor1->setSpeed(leftSpeed); //left
motor3->setSpeed(leftSpeed);
motor2->setSpeed(rightSpeed); //left
motor4->setSpeed(rightSpeed);
double width = pixy.blocks[trackedBlock].width;
if (width <= 5)
{
} else if (width < 20 && !running)
{
Serial.println("running");
motor1->run(FORWARD);
motor3->run(FORWARD);
motor2->run(FORWARD);
motor4->run(FORWARD);
running = true;
}
else if (width > 80 && !running)
{
Serial.println("running");
motor1->run(BACKWARD);
motor3->run(BACKWARD);
motor2->run(BACKWARD);
motor4->run(BACKWARD);
running = true;
}
else if (width >= 20 && width <= 80 && running) {
motor1->setSpeed(128);
motor2->setSpeed(128);
motor3->setSpeed(128);
motor4->setSpeed(128);
motor2->run(RELEASE);
motor4->run(RELEASE);
motor1->run(RELEASE);
motor3->run(RELEASE);
running = false;
}
}
}
}
//######### Subrutines ################################
// This function will set the speed
BLYNK_WRITE(V0)
{
int pinValue = param.asInt(); // assigning incoming value from pin V1 to a variable
// You can also use:
// String i = param.asStr();
// double d = param.asDouble();
Serial.print("V0 Slider value is: ");
Serial.println(pinValue);
motor1->setSpeed(pinValue);
motor2->setSpeed(pinValue);
motor3->setSpeed(pinValue);
motor4->setSpeed(pinValue);
}
// Motor 1 Forward
BLYNK_WRITE(V1)
{
int pinValue = param.asInt(); // assigning incoming value from pin V1 to a variable
Serial.print("Motor 1 Forward: ");
Serial.println(pinValue);
if(pinValue == 1) {
motor1->run(FORWARD);
motor3->run(FORWARD);
}
if(pinValue == 0) {
motor1->run(RELEASE);
motor3->run(RELEASE);
}
}
// Motor 2 Forward
BLYNK_WRITE(V2)
{
int pinValue = param.asInt(); // assigning incoming value from pin V1 to a variable
Serial.print("Motor 2 Forward: ");
Serial.println(pinValue);
if(pinValue == 1) {
motor2->run(FORWARD);
motor4->run(FORWARD);
}
if(pinValue == 0) {
motor2->run(RELEASE);
motor4->run(RELEASE);
}
}
// Motor 1 Backward
BLYNK_WRITE(V3)
{
int pinValue = param.asInt(); // assigning incoming value from pin V1 to a variable
Serial.print("Motor 1 Backward: ");
Serial.println(pinValue);
if(pinValue == 1) {
motor1->run(BACKWARD);
motor3->run(BACKWARD);
}
if(pinValue == 0) {
motor1->run(RELEASE);
motor3->run(RELEASE);
}
}
// Motor 2 Backward
BLYNK_WRITE(V4)
{
int pinValue = param.asInt(); // assigning incoming value from pin V1 to a variable
Serial.print("Motor 2 Backward: ");
Serial.println(pinValue);
if(pinValue == 1) {
motor2->run(BACKWARD);
motor4->run(BACKWARD);
}
if(pinValue == 0) {
motor2->run(RELEASE);
motor4->run(RELEASE);
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.