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