Arduino 101 + Pixy

Arduino101_Pixy.ino

//
// 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);    
  }
}