Vision Sensor targeting

I had a play around today with the vision sensor on one of my old demo robots. The goal was just to test some simple targeting code that could be used as a basis for a more complex program. The robot setup looks like this.

I obviously have a large target, but the concept would be the same no matter what size although the code would need to filter out more noise (meaning removing small intermittent objects) with something less well defined. I have the vision sensor mounted sideways so as the robot rotates the detected object will move vertically in the field of view. The targeting code looks like this.

#define TARGET  150

void
eventButtonY() {
  while(1) {
    // get latest objects
    int numberObjects = Vision1.takeSnapshot( SIG_1 );

    if( numberObjects > 0 ) {
      // make a copy
      vex::vision::object obj = Vision1.objects[0];

      if( obj.centerY != 0 && obj.width > 20 && obj.height > 20 ) {
        int error = obj.centerY - TARGET;

        // simple P control
        int drive = error;
        if( drive > 50 ) drive = 50;
        if( drive < -50 ) drive = -50;

        // object is found and centered
        if( obj.centerY > (TARGET-15) && obj.centerY < (TARGET+15) ) {
          driveTurn(0);
        }
        else {
          driveTurn( drive );
        }
      } 
    }
    else {
      // look for object
      driveTurn( 50 );
    }
    
    // pressing X stops loop
    if( Controller.ButtonX.pressing() ) {
      driveTurn(0);
      return;
    }
he field of view
    vex::this_thread::sleep_for(50);
  }
}

I trigger on a button, that’s why the function is called eventButtonY.

The code runs in a loop, I grab the latest objects found by the vision sensor, check we have at least one and it is large enough. Then there is a simple P loop trying to place the center of the object at a particular location in the field of view.

Here is the whole program

target_program.cpp
/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       James                                                     */
/*    Created:      16 Feb 2019                                               */
/*    Description:  V5 project                                                */
/*                                                                            */
/*----------------------------------------------------------------------------*/
#include "robot-config.h"

/*-----------------------------------------------------------------------------*/
/** @cond      name the motor ports                                            */
/*-----------------------------------------------------------------------------*/
#define LeftDrive1      0
#define LeftDrive2      1
#define RightDrive1     8
#define RightDrive2     9

vex::motor m_lb( LeftDrive1,  vex::gearSetting::ratio36_1, true );
vex::motor m_lf( LeftDrive2,  vex::gearSetting::ratio36_1, true );
vex::motor m_rf( RightDrive1, vex::gearSetting::ratio36_1 );
vex::motor m_rb( RightDrive2, vex::gearSetting::ratio36_1 );


vex::brain       Brain;
vex::competition Competition;
vex::controller  Controller;

using namespace vex;

int   screen_origin_x = 150;
int   screen_origin_y = 20;
int   screen_width    = 316;
int   screen_height   = 212;

// function to draw a single object
void
drawObject( vision::object &obj, vex::color c ) {
    int labelOffset = 0;
    
    Brain.Screen.setPenColor( vex::color::yellow );
    Brain.Screen.drawRectangle( screen_origin_x + obj.originX, screen_origin_y + obj.originY, obj.width, obj.height, c );
    Brain.Screen.setFont( vex::fontType::mono12 );
    
    if( obj.originX > 280 )
      labelOffset = -40;
    if( obj.originY > 10 )
      Brain.LCD.printAt( screen_origin_x + obj.originX + labelOffset, screen_origin_y + obj.originY-3, "Sig %o", obj.id );
    else
      Brain.LCD.printAt( screen_origin_x + obj.originX + labelOffset, screen_origin_y + obj.originY+10, "Sig %o", obj.id );
}

// function to draw all objects found
void
drawObjects( vision &v, vex::color c, bool clearScreen ) {
    if( clearScreen ) {
      Brain.Screen.setPenColor( vex::color::black );
      Brain.Screen.drawRectangle( screen_origin_x, screen_origin_y, screen_width, screen_height, vex::color::black );
    }
    
    for(int i=0;i<v.objectCount;i++)
      drawObject( v.objects[i], c );
}

bool turning = false;

void
driveTurn( int speed ) {
      int   drive_l_front = speed;
      int   drive_l_back  = speed;
      int   drive_r_front = -speed;
      int   drive_r_back  = -speed;

      if( speed != 0 )
        turning = true;
      else
        turning = false;
      m_lf.spin( vex::directionType::fwd, drive_l_front, vex::velocityUnits::rpm );
      m_lb.spin( vex::directionType::fwd, drive_l_back , vex::velocityUnits::rpm );
      m_rf.spin( vex::directionType::fwd, drive_r_front, vex::velocityUnits::rpm );
      m_rb.spin( vex::directionType::fwd, drive_r_back , vex::velocityUnits::rpm );
}

void
eventButtonA() {
  driveTurn( 50 );
}
void
eventButtonB() {
  driveTurn( -50 );
}
void
eventButtonX() {
  driveTurn( 0 );
}

#define TARGET  150

void
eventButtonY() {
  while(1) {
    // get latest objects
    int numberObjects = Vision1.takeSnapshot( SIG_1 );

    if( numberObjects > 0 ) {
      // make a copy
      vex::vision::object obj = Vision1.objects[0];

      if( obj.centerY != 0 && obj.width > 20 && obj.height > 20 ) {
        int error = obj.centerY - TARGET;

        // simple P control
        int drive = error;
        if( drive > 50 ) drive = 50;
        if( drive < -50 ) drive = -50;

        // object is found and centered
        if( obj.centerY > (TARGET-15) && obj.centerY < (TARGET+15) ) {
          driveTurn(0);
        }
        else {
          driveTurn( drive );
        }
      } 
    }
    else {
      // look for object
      driveTurn( 50 );
    }
    
    // pressing X stops loop
    if( Controller.ButtonX.pressing() ) {
      driveTurn(0);
      return;
    }

    vex::this_thread::sleep_for(50);
  }
}

int main() {        
    // Draw an area representing the vision sensor field of view
    Brain.Screen.clearScreen( vex::color::black );
    Brain.Screen.setPenColor( vex::color::green );
    Brain.Screen.drawRectangle( screen_origin_x-1, screen_origin_y-1, screen_width+2, screen_height+2 );

    Controller.ButtonA.pressed( eventButtonA );
    Controller.ButtonB.pressed( eventButtonB );
    Controller.ButtonX.pressed( eventButtonX );
    Controller.ButtonY.pressed( eventButtonY );

    while(1) {
      // request any objects with signature 1
      int numberObjects = Vision1.takeSnapshot( SIG_1 );
      
      Brain.Screen.setPenColor( vex::color::white );
      Brain.Screen.setFont( mono20 );
      Brain.Screen.setCursor( 2, 2 );
      Brain.Screen.print( "Sig 1 %2d", (int)numberObjects );
      Brain.Screen.setCursor( 3, 2 );
      Brain.Screen.print( "C %4d", Vision1.objects[0].centerY  );

      // draw any objects found
      drawObjects( Vision1, vex::color::blue, true );

      // run 10 times/second
      this_thread::sleep_for(100);      
    }   
}
7 Likes

@jpearman , does the vision API work properly with color codes? I always had issues trying to use the green + flag color as a color code.

1 Like