Fetch robot using a plain Arduino Uno, Pixy2 and two servo motors [Solved]

gambi_arra · March 23, 2020, 5:04pm

Dear Android community!

I am trying to build a fetch robot using a Arduino Uno, Pixy2 and two continuous servo motors.

I found this sketch.

// fetch robot pixy2
include <ESP32_Servo.h>
#include <Pixy2I2C.h>
#include "esp32-hal-ledc.h"
Pixy2I2C pixy;

#define CENTER 102
#define DEADZONE 8

int motor_pins[] = {26,27,9,10};

Servo gripper;
int servo_pin = 25;
int pos;
int state = 1;

const int freq = 490, resolution = 8;

void setup()
{
  Serial.begin(115200);
  Serial.print("Starting...\n");
 
  pixy.init();
  gripper.attach(servo_pin);
  for(int i=0;i<4;i++){
    ledcAttachPin(motor_pins[i],i+2);
    ledcSetup(i+2,freq,resolution);
    
  }
  set_gripper(0);
  delay(2000);
  set_gripper(1);
}

void loop()
{
  int i;
  // grab blocks!
  pixy.ccc.getBlocks();
 
  // If there are detect blocks, print them!
  if (pixy.ccc.numBlocks)
  {
    //Serial.print("Detected ");
    //Serial.println(pixy.ccc.numBlocks);
    for (i=0; i<pixy.ccc.numBlocks; i++)
    {
      /*Serial.print(pixy.ccc.blocks[i].m_width);
      Serial.print(" ");
      Serial.println(pixy.ccc.blocks[i].m_height);
      /*Serial.print("  block ");*/
      //Serial.print(i);
      //Serial.print(": ");
      //pixy.ccc.blocks[i].print();
      if(pixy.ccc.blocks[i].m_width > 220){
        if(pixy.ccc.blocks[i].m_x >= CENTER-DEADZONE || pixy.ccc.blocks[i].m_x <= CENTER+DEADZONE){
          Serial.print("Grabbed at ");
        Serial.print(pixy.ccc.blocks[i].m_x);
        Serial.print(",");
        Serial.println(pixy.ccc.blocks[i].m_width);
        set_gripper(2);
        move(0);
        state = 0;
        delay(1000);
        move(4);
        delay(750);
        move(0);
        set_gripper(1);
        while(1);
        }
        
      }
    Serial.print(pixy.ccc.blocks[i].m_x);
    Serial.print(",");
    Serial.println(pixy.ccc.blocks[i].m_width);
    }
    if(state==1){
    if(pixy.ccc.blocks[i].m_x >= CENTER-DEADZONE || pixy.ccc.blocks[i].m_x <= CENTER+DEADZONE){
      move(3);
      delay(2);
      move(0);
    }
    else if(pixy.ccc.blocks[i].m_x > CENTER){
      //while(pixy.ccc.blocks[i].m_x > CENTER){
        move(1);
        delay(1);
        move(0);
      //}
    }
    
    else if(pixy.ccc.blocks[i].m_x < CENTER){
      //while(pixy.ccc.blocks[i].m_x < CENTER){
        move(2);
        delay(1);
        move(0);
      //}
    }
  }  
  }
  if(Serial.available()>0){
    pos = Serial.parseInt();
    Serial.println(pos);
    if(pos != 0){
      gripper.write(pos);
    }
  }
 
}

void move(int dir){
  switch(dir){
    case 0: //stop
      ledcWrite(2, 0);
      ledcWrite(3, 0);
      ledcWrite(4, 0);
      ledcWrite(5, 0);
      break;
    case 1: //left
      ledcWrite(2, 120);
      ledcWrite(3, 0);
      ledcWrite(4, 120);
      ledcWrite(5, 0);
      break;
    case 2: //right
      ledcWrite(2, 0);
      ledcWrite(3, 120);
      ledcWrite(4, 0);
      ledcWrite(5, 120);
      break;
    case 3: //forward
      ledcWrite(2, 0);
      ledcWrite(3, 180);
      ledcWrite(4, 180);
      ledcWrite(5, 0);
      break;
    case 4: //back
      ledcWrite(2, 120);
      ledcWrite(3, 0);
      ledcWrite(4, 0);
      ledcWrite(5, 120);
      break;
  }
}

void set_gripper(int state){
  switch(state){
    case 0:
      gripper.write(180);//closed
      break;
    case 1:
      gripper.write(0);//open
      break;
    case 2:
      gripper.write(60);//grip
      break;
  }
}

But unfortunately this project uses dc-gear-motors and a ESP32-controller. I am also not sure, if the wiring-scheme is correct.

Maybe someone could help me modify the sketch?

Thank you very much for any help or hints!

anon73444976 · March 23, 2020, 8:09pm

Use Servo.h, define two more servos for the motors, define the appropriate pins for the ex-servos.

Avoid the SPI pins, because the Pixy will want those.

Rewrite the motor controls to use the ex-servos.

I don't imagine it should take too long.

vinceherman · March 23, 2020, 9:18pm

gambi_arra:
Maybe someone could help me modify the sketch?

We love doing this!
Follow AWOL's advice, post your best attempt at changing the code to talk to your CR servos and let us know how it works.
Assuming you do not hit the nail on the head with the first swing, tells us what you expected to happen and what actually happened.

OH!!! Proper use of code tags on your first post!!! Karma++

zoomkat · March 23, 2020, 9:45pm

Below is some basic servo test code you can use to verify your servos are wired up correctly and see how a continuous rotation servos operate.

// zoomkat 7-30-10 serial servo test
// type servo position 0 to 180 in serial monitor
// for writeMicroseconds, use a value like 1500
// Powering a servo from the arduino usually *DOES NOT WORK*.

String readString;
#include <Servo.h> 
Servo myservo;  // create servo object to control a servo 

void setup() {
  Serial.begin(9600);
  myservo.attach(9);
}

void loop() {

  while (Serial.available()) {

    if (Serial.available() >0) {
      char c = Serial.read();  //gets one byte from serial buffer
      readString += c; //makes the string readString
      delay(3);
    } 
  }

  if (readString.length() >0) {
    Serial.println(readString);
    int n = readString.toInt();
    Serial.println(n);
    myservo.writeMicroseconds(n);
    //myservo.write(n);
    readString="";
  } 
}

gambi_arra · March 24, 2020, 6:16am

Thank you! I'll try my best.

gambi_arra · March 24, 2020, 8:52pm

I modified the code and also included some more Pixy2 libraries since the orignal sketch uses the 'pixy.ccc.getBlocks' function.

When I tried to compile the new code I got

In file included from /home/.../fetch_robot_pixy_servo_v3/fetch_robot_pixy_servo_v3.ino:2:0:
/root/Arduino/libraries/Pixy2/Pixy2CCC.h: In member function 'int8_t Pixy2CCC<LinkType>::getBlocks(bool, uint8_t, uint8_t)':
/root/Arduino/libraries/Pixy2/Pixy2CCC.h:124:32: error: 'PIXY_TYPE_RESPONSE_ERROR' was not declared in this scope
       else if (m_pixy->m_type==PIXY_TYPE_RESPONSE_ERROR)
                                ^~~~~~~~~~~~~~~~~~~~~~~~

Among others.

So I headed to Pixy2CCC.h and declared those variables.

#ifndef _PIXY2CCC_H
#define _PIXY2CCC_H

#define CCC_MAX_SIGNATURE                   7

#define CCC_RESPONSE_BLOCKS               0x21
#define CCC_REQUEST_BLOCKS                  0x20

#define PIXY_TYPE_RESPONSE_ERROR	   0x03
#define PIXY_RESULT_BUSY                          0x03
#define PIXY_RESULT_PROG_CHANGING    0x03
#define PIXY_RESULT_ERROR                       0x03

I also got

       ^~~~~~~
       Serial
/root/Arduino/libraries/Pixy2/Pixy2UART.h:34:7: error: 'Serial1' was not declared in this scope
       Serial1.begin(arg);      

       ^~~~~~~
/root/Arduino/libraries/Pixy2/Pixy2UART.h:34:7: note: suggested alternative: 'Serial'
       Serial1.begin(arg);

So I also changed "Serial1" to "Serial" in Pixy2UART.h.

Arduino IDE is now compiling without any error messages, but the servos still wont move.

Pixy2 seems to recognize the objects/signatures I set up in PixyMon but the only output I get on Serial Monitor is

"Starting..."

Any ideas?

Here is my code with the servo commands

#include <Pixy2CCC.h>
#include <Pixy2UART.h>
#include <Pixy2I2C.h>
#include <PIDLoop.h>
#include <TPixy2.h>
#include <Pixy2Video.h>
#include <Pixy2Line.h>
#include <Pixy2.h>

#include <Servo.h>


Pixy2I2C pixy;

#define CENTER 102
#define DEADZONE 8

Servo right_servo;
Servo left_servo;
Servo gripper_servo;

int right_servo_pin = 5;
int left_servo_pin = 6;
int gripper_servo_pin = 9;
int pos;
int state = 1;

const int freq = 490, resolution = 8;

void setup()
{
  Serial.begin(115200);
  Serial.print("Starting...\n");
  
  pixy.init();
  
  right_servo.attach(right_servo_pin);
  left_servo.attach(right_servo_pin);
  gripper_servo.attach(gripper_servo_pin);
  
  set_gripper_servo(0);
  delay(2000);
  set_gripper_servo(1);
}

void loop()
{ 
  int i; 
  // grab blocks!
  pixy.ccc.getBlocks();
  
  // If there are detect blocks, print them!
  if (pixy.ccc.numBlocks)
  {
    //Serial.print("Detected ");
    //Serial.println(pixy.ccc.numBlocks);
    for (i=0; i<pixy.ccc.numBlocks; i++)
    {
      /*Serial.print(pixy.ccc.blocks[i].m_width);
      Serial.print(" ");
      Serial.println(pixy.ccc.blocks[i].m_height);
      /*Serial.print("  block ");*/
      //Serial.print(i);
      //Serial.print(": ");
      //pixy.ccc.blocks[i].print();
      if(pixy.ccc.blocks[i].m_width > 220){
        if(pixy.ccc.blocks[i].m_x >= CENTER-DEADZONE || pixy.ccc.blocks[i].m_x <= CENTER+DEADZONE){
          Serial.print("Grabbed at ");
        Serial.print(pixy.ccc.blocks[i].m_x);
        Serial.print(",");
        Serial.println(pixy.ccc.blocks[i].m_width);
        set_gripper_servo(2);
        right_servo.write(91);  //Stop
        left_servo.write(93); 
        state = 0;
        delay(1000);
        right_servo.write(180);  //Back
        left_servo.write(-180); 
        delay(750);
        right_servo.write(91);  //Stop
        left_servo.write(93); 
        set_gripper_servo(1);
        while(1);
        }
        
      }
    Serial.print(pixy.ccc.blocks[i].m_x);
    Serial.print(",");
    Serial.println(pixy.ccc.blocks[i].m_width);
    }
    if(state==1){
    if(pixy.ccc.blocks[i].m_x >= CENTER-DEADZONE || pixy.ccc.blocks[i].m_x <= CENTER+DEADZONE){
      right_servo.write(-180);  //Forward
      left_servo.write(180); 
      delay(2);
      right_servo.write(91);  //Stop
      left_servo.write(93); 
    }
    else if(pixy.ccc.blocks[i].m_x > CENTER){
      //while(pixy.ccc.blocks[i].m_x > CENTER){
        right_servo.write(-180);  //Left
        left_servo.write(-180); 
        delay(1);
        right_servo.write(91);  //Stop
        left_servo.write(93); 
      //}
    }
    
    else if(pixy.ccc.blocks[i].m_x < CENTER){
      //while(pixy.ccc.blocks[i].m_x < CENTER){
        right_servo.write(180);  //Right
        left_servo.write(180); 
        delay(1);
        right_servo.write(91);  //Stop
        left_servo.write(93); 
      //}
    }
  }  
  }
  if(Serial.available()>0){
    pos = Serial.parseInt();
    Serial.println(pos);
    if(pos != 0){
      gripper_servo.write(pos);
    }
  }
  
}

void set_gripper_servo(int state){
  switch(state){
    case 0: 
      gripper_servo.write(180);//closed
      break;
    case 1:
      gripper_servo.write(0);//open
      break;
    case 2:
      gripper_servo.write(60);//grip
      break;
  }
}

Changelog (2020-03-23_v1)

- removed Zumo libraries
- added servo variables
- renamed ‘gripper” to ‘gripper_servo’

- removed

int motor_pins[] = {26,27,9,10};


 for(int i=0;i<4;i++){
    ledcAttachPin(motor_pins[i],i+2);
    ledcSetup(i+2,freq,resolution);  
  }


void move(int dir){
  switch(dir){
    case 0: //stop
      ledcWrite(2, 0);
      ledcWrite(3, 0);
      ledcWrite(4, 0);
      ledcWrite(5, 0);
      break;
    case 1: //left
      ledcWrite(2, 120);
      ledcWrite(3, 0);
      ledcWrite(4, 120);
      ledcWrite(5, 0);
      break;
    case 2: //right
      ledcWrite(2, 0);
      ledcWrite(3, 120);
      ledcWrite(4, 0);
      ledcWrite(5, 120);
      break;
    case 3: //forward
      ledcWrite(2, 0);
      ledcWrite(3, 180);
      ledcWrite(4, 180);
      ledcWrite(5, 0);
      break;
    case 4: //back
      ledcWrite(2, 120);
      ledcWrite(3, 0);
      ledcWrite(4, 0);
      ledcWrite(5, 120);
      break;
  }
}

anon73444976 · March 24, 2020, 8:55pm

Why are you messing with the Pixy libraries?

Hopefully, you can now see why we ask you to use code tags when posting code

gambi_arra · March 24, 2020, 9:06pm

I messed with it because I got

'PIXY_TYPE_RESPONSE_ERROR' was not declared in this scope

when I tried to compile the sketch.

It seemed strange, that there should be a bug in the library but that was the Log I got from the IDE.

anon73444976 · March 24, 2020, 9:10pm

I only have a Pixy, but I've never had to mess with the libraries.

Are you sure that you have:
a) the correct library for your processor?
b) the library is even compatible with your processor?

gambi_arra · March 24, 2020, 9:55pm

Right servo is turning now.
I took a look at the "hello_world" demo, added the libraries again and renamed "Pixy2I2C pixy;" to "Pixy2 pixy;"
Serial monitor is also giving me some output now.

I'll get back on this tomorrow.

v4

#include <Pixy2CCC.h>
#include <Pixy2UART.h>
#include <Pixy2I2C.h>
#include <PIDLoop.h>
#include <TPixy2.h>
#include <Pixy2Video.h>
#include <Pixy2Line.h>
#include <Pixy2.h>
#include <Pixy2SPI_SS.h>



#include <Servo.h>


Pixy2 pixy;

#define CENTER 102
#define DEADZONE 8

Servo right_servo;
Servo left_servo;
Servo gripper_servo;

int right_servo_pin = 5;
int left_servo_pin = 6;
int gripper_servo_pin = 9;
int pos;
int state = 1;

const int freq = 490, resolution = 8;

void setup()
{
  Serial.begin(115200);
  Serial.print("Starting...\n");
  
  pixy.init();
  
  right_servo.attach(right_servo_pin);
  left_servo.attach(right_servo_pin);
  gripper_servo.attach(gripper_servo_pin);
  
  set_gripper_servo(0);
  delay(2000);
  set_gripper_servo(1);
}

void loop()
{ 
  int i; 
  // grab blocks!
  pixy.ccc.getBlocks();
  
  // If there are detect blocks, print them!
  if (pixy.ccc.numBlocks)
  {
    //Serial.print("Detected ");
    //Serial.println(pixy.ccc.numBlocks);
    for (i=0; i<pixy.ccc.numBlocks; i++)
    {
      /*Serial.print(pixy.ccc.blocks[i].m_width);
      Serial.print(" ");
      Serial.println(pixy.ccc.blocks[i].m_height);
      /*Serial.print("  block ");*/
      //Serial.print(i);
      //Serial.print(": ");
      //pixy.ccc.blocks[i].print();
      if(pixy.ccc.blocks[i].m_width > 220){
        if(pixy.ccc.blocks[i].m_x >= CENTER-DEADZONE || pixy.ccc.blocks[i].m_x <= CENTER+DEADZONE){
          Serial.print("Grabbed at ");
        Serial.print(pixy.ccc.blocks[i].m_x);
        Serial.print(",");
        Serial.println(pixy.ccc.blocks[i].m_width);
        set_gripper_servo(2);
        right_servo.write(91);  //Stop
        left_servo.write(93); 
        state = 0;
        delay(1000);
        right_servo.write(180);  //Back
        left_servo.write(-180); 
        delay(750);
        right_servo.write(91);  //Stop
        left_servo.write(93); 
        set_gripper_servo(1);
        while(1);
        }
        
      }
    Serial.print(pixy.ccc.blocks[i].m_x);
    Serial.print(",");
    Serial.println(pixy.ccc.blocks[i].m_width);
    }
    if(state==1){
    if(pixy.ccc.blocks[i].m_x >= CENTER-DEADZONE || pixy.ccc.blocks[i].m_x <= CENTER+DEADZONE){
      right_servo.write(-180);  //Forward
      left_servo.write(180); 
      delay(2);
      right_servo.write(91);  //Stop
      left_servo.write(93); 
    }
    else if(pixy.ccc.blocks[i].m_x > CENTER){
      //while(pixy.ccc.blocks[i].m_x > CENTER){
        right_servo.write(-180);  //Left
        left_servo.write(-180); 
        delay(1);
        right_servo.write(91);  //Stop
        left_servo.write(93); 
      //}
    }
    
    else if(pixy.ccc.blocks[i].m_x < CENTER){
      //while(pixy.ccc.blocks[i].m_x < CENTER){
        right_servo.write(180);  //Right
        left_servo.write(180); 
        delay(1);
        right_servo.write(91);  //Stop
        left_servo.write(93); 
      //}
    }
  }  
  }
  if(Serial.available()>0){
    pos = Serial.parseInt();
    Serial.println(pos);
    if(pos != 0){
      gripper_servo.write(pos);
    }
  }
  
}

void set_gripper_servo(int state){
  switch(state){
    case 0: 
      gripper_servo.write(180);//closed
      break;
    case 1:
      gripper_servo.write(0);//open
      break;
    case 2:
      gripper_servo.write(60);//grip
      break;
  }
}

gambi_arra · March 25, 2020, 6:30am

In the meanwhile...

TheMemberFormerlyKnownAsAWOL:
Why are you messing with the Pixy libraries?

That's a good question. Any ideas, why the error message showed up when compiling the sketch?
Will also try to get some infos directly from the Pixy-Team.

gambi_arra · March 25, 2020, 12:05pm

Found the declaration in another file

github.com

charmedlabs/pixy2/blob/master/src/common/inc/pixydefs.h

//
// 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
// cmucam@cs.cmu.edu. Such licensing terms are available for
// all portions of the Pixy codebase presented here.
//
// end license header
//

#ifndef __PIXYDEFS_H__
#define __PIXYDEFS_H__

#include "libusb.h"

This file has been truncated. show original

#define PIXY_TYPE_RESPONSE_ERROR             0x03

Do I have to include pixydefs.h manually in the library and my sketch? In other words: why is Pixy2CCC.h not referring to this declaration?

gambi_arra · March 26, 2020, 3:16pm

Fixed some code

right_servo.attach(right_servo_pin);
left_servo.attach(left_servo_pin);

gambi_arra · March 26, 2020, 3:44pm

Almost there...

gambi_arra · March 26, 2020, 3:49pm

Almost there...

v5

#include <ZumoMotors.h>
#include <Pixy2CCC.h>
#include <ZumoBuzzer.h>
#include <Pixy2UART.h>
#include <Pixy2I2C.h>
#include <PIDLoop.h>
#include <TPixy2.h>
#include <Pixy2Video.h>
#include <Pixy2Line.h>
#include <Pixy2.h>
#include <Pixy2SPI_SS.h>


#include <Servo.h>


Pixy2 pixy;

#define CENTER 102
#define DEADZONE 8

Servo right_servo;
Servo left_servo;
Servo gripper_servo;

int right_servo_pin = 5;
int left_servo_pin = 6;
int gripper_servo_pin = 9;
int pos;
int state = 1;

const int freq = 490, resolution = 8;

void setup()
{
  Serial.begin(115200);
  Serial.print("Starting...\n");
 
  pixy.init();
 
  right_servo.attach(right_servo_pin);
  left_servo.attach(left_servo_pin);
  gripper_servo.attach(gripper_servo_pin);
 
  set_gripper_servo(0);
  delay(2000);
  set_gripper_servo(1);
}

void loop()
{
  int i;
  // grab blocks!
  pixy.ccc.getBlocks();
 
  // If there are detect blocks, print them!
  if (pixy.ccc.numBlocks)
  {
    //Serial.print("Detected ");
    //Serial.println(pixy.ccc.numBlocks);
    for (i=0; i<pixy.ccc.numBlocks; i++)
    {
      /*Serial.print(pixy.ccc.blocks[i].m_width);
      Serial.print(" ");
      Serial.println(pixy.ccc.blocks[i].m_height);
      /*Serial.print("  block ");*/
      //Serial.print(i);
      //Serial.print(": ");
      //pixy.ccc.blocks[i].print();
      if(pixy.ccc.blocks[i].m_width > 220){
        if(pixy.ccc.blocks[i].m_x >= CENTER-DEADZONE || pixy.ccc.blocks[i].m_x <= CENTER+DEADZONE){
          Serial.print("Grabbed at ");
        Serial.print(pixy.ccc.blocks[i].m_x);
        Serial.print(",");
        Serial.println(pixy.ccc.blocks[i].m_width);
        set_gripper_servo(2);
        right_servo.write(91);  //Stop
        left_servo.write(93);
        state = 0;
        delay(1000);
        right_servo.write(180);  //Back
        left_servo.write(-180);
        delay(750);
        right_servo.write(91);  //Stop
        left_servo.write(93);
        set_gripper_servo(1);
        while(1);
        }
       
      }
    Serial.print(pixy.ccc.blocks[i].m_x);
    Serial.print(",");
    Serial.println(pixy.ccc.blocks[i].m_width);
    }
    if(state==1){
    if(pixy.ccc.blocks[i].m_x >= CENTER-DEADZONE || pixy.ccc.blocks[i].m_x <= CENTER+DEADZONE){
      right_servo.write(-180);  //Forward
      left_servo.write(180);
      delay(100);
      right_servo.write(91);  //Stop
      left_servo.write(93);
      delay(20);
    }
    else if(pixy.ccc.blocks[i].m_x > CENTER){
      //while(pixy.ccc.blocks[i].m_x > CENTER){
        right_servo.write(-180);  //Left
        left_servo.write(-180);
        delay(20);
        right_servo.write(91);  //Stop
        left_servo.write(93);
        delay(20);
      //}
    }
   
    else if(pixy.ccc.blocks[i].m_x < CENTER){
      //while(pixy.ccc.blocks[i].m_x < CENTER){
        right_servo.write(180);  //Right
        left_servo.write(180);
        delay(20);
        right_servo.write(91);  //Stop
        left_servo.write(93);
        delay(20);
      //}
    }
  } 
  }
  if(Serial.available()>0){
    pos = Serial.parseInt();
    Serial.println(pos);
    if(pos != 0){
      gripper_servo.write(pos);
    }
  }
 
}

void set_gripper_servo(int state){
  switch(state){
    case 0:
      gripper_servo.write(180);//closed
      break;
    case 1:
      gripper_servo.write(0);//open
      break;
    case 2:
      gripper_servo.write(60);//grip
      break;
  }
}

Topic		Replies	Views
Beginner's Robotic Arm Robotics	43	19476	May 6, 2021
Programming Servo control for Model Railroad Turnouts Programming	103	34239	May 5, 2021
robotic arm problem help me please (4dof) Programming	28	2939	May 20, 2021
Servo motors on Arduino micro Motors, Mechanics, Power and CNC	17	274	August 29, 2024
Please help me... General Guidance	11	2850	May 5, 2021

Fetch robot using a plain Arduino Uno, Pixy2 and two servo motors [Solved]

Related topics