RobotDemo(void)
{
kicker_in_motion = false;
sol = new Solenoid(2);
rightstick = new Joystick(1);
leftstick = new Joystick(2);
lonelystick = new Joystick (3);
Motor1=new Jaguar(1);
Motor2=new Jaguar(2);
Motor3=new Jaguar(3);
Motor4=new Jaguar(4);
BallGathererMotor9 = new Jaguar(9);
myRobot=new RobotDrive(Motor1,Motor2,Motor4,Motor3);
rlyLED=new Relay(8,Relay::kForwardOnly);
cam = &AxisCamera::GetInstance("10.8.12.11");
cam->WriteResolution(AxisCameraParams::kResolution_160x120);
myRobot->SetExpiration(0.5);
ControllBox = & DriverStation::GetInstance()->GetEnhancedIO();
shooter1 = new Jaguar(5); //front left
shooter2 = new Jaguar(6);
shooter3 = new Jaguar(7);
shooter4 = new Jaguar(8);
//shooterDin = new DigitalInput(1);
shootercontador = new Counter(1);
shootercontador->Start();
shooterspeedTask = new Task("ShooterSpeed",(FUNCPTR)&shooterspeedloop);
kickerTask = new Task ("Kicker", (FUNCPTR)&kickerloop);
Upperlimit = new DigitalInput(3);
Lowerlimit = new DigitalInput(2);
kickermotor = new Relay (6, Relay::kBothDirections);
BridgeBootMotor10 = new Jaguar(10);
kicker_cancel = false;
kicker_down = false;
}
void RobotDemo::camera_test()
{
#if CAMERA
image = camera->GetImage();
cout << image << endl;
delete image;
#endif
}
void AxisRenderer::render(const AxisCamera &camera, QGLShaderProgram &program, QGLBuffer &vertBuffer, QGLBuffer &indexBuffer, bool selected)
{
qDebug()<<"[AxisRender] start of render";
initializeGLFunctions();
//bind the program for use
if(!program.bind()){
QMessageBox::critical(0, "Error", "Could not bind program for use");
qFatal("Could not bind program for use");
}
qDebug()<<"[AxisRender] Drawing";
//set up shader locations
program.bind();
GLint vertLoc = program.attributeLocation("vertex");
Q_ASSERT(vertLoc != -1);
GLint colorLoc = program.uniformLocation("color");
Q_ASSERT(colorLoc != -1);
GLint mvpLoc = program.uniformLocation("modelToCamera");
Q_ASSERT(mvpLoc != -1);
//set default color to a cyan
QVector4D color(0.0f, .5f, 1.0f, 1.0f);
//modify the color for selected geometry
if(selected){
qDebug()<<"[AxisRender] adding selection color";
color = color + QVector4D(1.0f, 0.0f, 0.0f, 0.0f);
}
//set the uniform values
program.setUniformValueArray(colorLoc, &color, 1);
program.setUniformValueArray(mvpLoc, &camera.getProjMatrix(), 1);
//bind and set the vertex buffer for attribute
vertBuffer.bind();
program.enableAttributeArray(vertLoc);
program.setAttributeBuffer(vertLoc, GL_FLOAT, 0, 4);
//bind the index buffer for the draw call
indexBuffer.bind();
qDebug()<<"[AxisRender] Drawing";
//draw the index buffer NOTE: Hard set to 24 values for a box!
glDrawElements(GL_LINES, 24, GL_UNSIGNED_INT, NULL);
}
void RobotInit() override {
// create an image
// open the camera at the IP address assigned. This is the IP address that the camera
// can be accessed through the web interface.
camera = new AxisCamera("10.19.67.11");
stick = new jankyXboxJoystick (0);
camera->WriteResolution (AxisCamera::kResolution_320x240);
camera->WriteCompression(30);
camera->WriteRotation(AxisCamera::kRotation_0);
camera->WriteMaxFPS(15);
camera->WriteColorLevel(25);
camera->WriteBrightness(50);
camera->WriteWhiteBalance(AxisCamera::kWhiteBalance_Automatic);
camera->WriteExposureControl(AxisCamera::kExposureControl_Automatic);
}
void AxisRenderer::drawBrush(const M3DEditLevel::Box &brush, QGLShaderProgram &program, const AxisCamera &camera)
{
QVector<QVector3D> temp = brush.getVerticies();
QVector<GLfloat> verts;
//fill vector with brush verticies
for(int i = 0; i < temp.size(); ++i){
QVector3D vert = temp[i];
verts.push_back(vert.x());
verts.push_back(vert.y());
verts.push_back(vert.z());
verts.push_back(1.0f);
}
QVector<unsigned int> indicies = brush.getLineIndex();
//bind brush buffer and fill with verticies
QGLBuffer brushBuffer(QGLBuffer::VertexBuffer);
brushBuffer.create();
brushBuffer.bind();
brushBuffer.allocate(verts.data(), sizeof(GLfloat) * verts.size());
//bind index buffer and fill with indicies
QGLBuffer brushIndex(QGLBuffer::IndexBuffer);
brushIndex.create();
brushIndex.bind();
brushIndex.allocate(indicies.data(), sizeof(unsigned int) * indicies.size());
//set up uniforms
program.bind();
program.setUniformValueArray("color", &QVector4D(1.0f, 1.0f, 1.0f, 1.0f), 1);
program.setUniformValueArray("modelToCamera", &camera.getProjMatrix(), 1);
//set up buffers
brushBuffer.bind();
program.enableAttributeArray("vertex");
program.setAttributeBuffer("vertex", GL_FLOAT, 0, 4);
//draw brush. NOTE: Fixed 24 indicies for a box only!
brushIndex.bind();
glDrawElements(GL_LINES, 24, GL_UNSIGNED_INT, 0);
}
void AxisRenderer::drawCamLine(QVector3D from, QVector3D to, QGLShaderProgram &program, const AxisCamera &camera)
{
const GLfloat color[] ={1.0f, 0.0f, 0.0f, 1.0f}; //default red color
//temp vector to hold line verticies
std::vector<GLfloat> bufferData;
//from verticies
bufferData.push_back(from.x());
bufferData.push_back(from.y());
bufferData.push_back(from.z());
bufferData.push_back(1.0f);
//to verticies
bufferData.push_back(to.x());
bufferData.push_back(to.y());
bufferData.push_back(to.z());
bufferData.push_back(1.0f);
program.bind();
//create and fill line buffer with verticies
QGLBuffer line(QGLBuffer::VertexBuffer);
line.create();
line.bind();
line.allocate(bufferData.data(), sizeof(GLfloat) * 8);
//get locations
GLuint colorLoc = program.uniformLocation("color");
Q_ASSERT(colorLoc != -1);
GLuint mvpLoc = program.uniformLocation("modelToCamera");
Q_ASSERT(mvpLoc != -1);
//setup line for drawing
line.bind();
program.enableAttributeArray("vertex");
program.setAttributeBuffer("vertex", GL_FLOAT, 0, 4);
program.setUniformValueArray(colorLoc, color, 1, 4);
program.setUniformValueArray(mvpLoc, &(camera.getProjMatrix()), 1);
glDrawArrays(GL_LINES, 0, 2);
}
RobotDemo()
{
#if YEAR_2013
drive = new RobotDrive(left_drive_motor_A_PWM, left_drive_motor_B_PWM,
right_drive_motor_A_PWM, right_drive_motor_B_PWM);
drive->SetExpiration(15);
drive->SetSafetyEnabled(false);
#endif
drive->SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
drive->SetInvertedMotor(RobotDrive::kRearRightMotor, true);
drive->SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
//Joystick
//ds = new DriverStation();
drive_stick_sec = new Joystick(right_stick);
drive_stick_prim = new Joystick(left_stick);
operator_stick = new Joystick(operator_joystick);
//Motors
#if JAGUAR_SWITCH
shooter_motor_front = new Jaguar(shooter_front_motor);
shooter_motor_back = new Jaguar(shooter_back_motor);
#else
shooter_motor_front = new Talon(shooter_front_motor);
shooter_motor_back = new Talon(shooter_back_motor);
#endif
#if DUMB_DRIVE_CODE
left_drive_motor_A = new Victor(left_drive_motor_A_PWM);
left_drive_motor_B = new Victor(left_drive_motor_B_PWM);
right_drive_motor_A = new Victor(right_drive_motor_A_PWM);
right_drive_motor_B = new Victor(right_drive_motor_B_PWM);
#endif
#if YEAR_2013
climbing_motor = new Victor(climbing_motor_PWM);
#endif
//limit switches
top_claw_limit_switch = new DigitalInput(top_claw_limit_switch_port);
bottom_claw_limit_switch = new DigitalInput(
bottom_claw_limit_switch_port);
//Encoders
front_shooter_encoder = new Encoder(shooter_motor_front_encoder_A_port,
shooter_motor_front_encoder_B_port, false);
back_shooter_encoder = new Encoder(shooter_motor_back_encoder_A_port,
shooter_motor_back_encoder_B_port, false);
//solenoids
shooter_angle_1 = new Solenoid(SHOOTER_ANGLE_SOLENOID_1);
shooter_angle_2 = new Solenoid(SHOOTER_ANGLE_SOLENOID_2);
shooter_fire_piston_A = new Solenoid(shooter_fire_piston_solenoid_A);
shooter_fire_piston_B = new Solenoid(shooter_fire_piston_solenoid_B);
//Timers
shooter_piston_timer = new Timer();
VC_timer = new Timer();
loop_time_measure_timer = new Timer();
shooter_reset = new Timer();
pid_code_timer = new Timer();
autonomous_timer = new Timer();
error_timer = new Timer();
retraction_timer = new Timer();
override_timer = new Timer();
stabilizing_timer = new Timer();
shooter_stop_timer = new Timer();
//Compressor
compressor1 = new Compressor(PRESSURE_SWITCH, compressor_enable);
#if CAMERA
//Camera
camera = &(AxisCamera::GetInstance(AxisCameraIpAddr));
camera->WriteResolution(AxisCamera::kResolution_640x480);
AxisCamera::GetInstance();
#endif
//Function starter
compressor1 ->Start();
//float RPS;
//PIDController pid1 (0.1 , 0.001 ,0.0 , &RPS , test_motor );
loop_time_measure_timer ->Start();
VC_timer ->Start();
//Variable Initialization
arcadedrive = true;
test_encoder_value = 0;
total_test_encoder_value = 0;
old_RPS = 0;
new_RPS = 0;
second_count = 0;
set_speed = 0.5;
cycle_counter = 0;
//test_motor ->Set(set_speed);
// float RPS;
//int old_encoder_value;
//double old_timer;
speed2 = 0.0;
test_speed = 0.3;
button = false;
pickup_on = false;
switch_on = false;
kicker_piston_on = false;
kicker_button_on = false;
average_counter = 0;
counter = 1;
number = 1;
additive_error = 0;
prev_error_front = 0;
prev_desired_RPS = 0;
ROC = 0;
claw_ = false;
claw_go_down = false;
constant_ = false;
constant_desired_RPS = false;
divided = 0;
//RPS_encoder_1 = new Encoder(9, 10);// for 2012 robot
shooter_motor_back_RPS = shooter_motor_back->Get();
//RPS_encoder_1 -> SetDistancePerPulse(1 / 250);//TODO figure out how this works
first_press = true;
test_RPS = false;
desired_RPS_control = 0.0;
slow_control = 0;
//RPS_encoder_1 ->Start();
pid_code_timer ->Start();
front_shooter_encoder->Start();
back_shooter_encoder->Start();
autonomous_timer ->Start();
error_timer ->Start();
retraction_timer->Start();
stabilizing_timer->Start();
override_timer->Start();
}
bool tracking (bool use_alternate_score) //camera tracking function
{
//takes one camera frame and turns towards tallest target
//returns true if target is within deadzone, returns false otherwise
Threshold tapeThreshold(0, 255, 0, 90, 220, 255); //red hsl as of 20110303, this is the hue, saturation and luminosicity ranges that we want
BinaryImage *tapePixels;//
Image *convexHull;
BinaryImage *convexHullBinaryImage;
ParticleAnalysisReport par;//analyzed blob (pre convex hull)
ParticleAnalysisReport convexpar;// ONE filled-in blob
vector<ParticleAnalysisReport>* pars;//where many analyzed blob goes (pre)
vector<ParticleAnalysisReport>* convexpars; //where MANY filled-in blobs go
bool foundAnything = false;
double best_score = 120;
double best_speed;
double particle_score;
ImageType t;
int bs;
img = cam->GetImage();
printf("cam->GetImage() returned frame %d x %d\n",img->GetWidth(),img->GetHeight());
tapePixels = img->ThresholdHSL(tapeThreshold);
imaqGetBorderSize(tapePixels->GetImaqImage(),&bs);
imaqGetImageType(tapePixels->GetImaqImage(),&t);
convexHull = imaqCreateImage(t,bs);
convexHullBinaryImage = new BinaryImageWrapper(convexHull);
convexHullBinaryImage->GetOrderedParticleAnalysisReports();
//tapePixels = img->ThresholdHSL(int 0,int 50,int -100,int -50,int luminenceLow,int luminanceHigh);
pars = tapePixels->GetOrderedParticleAnalysisReports();
imaqConvexHull(convexHull,tapePixels->GetImaqImage(),true);
convexHullBinaryImage = new BinaryImageWrapper(convexHull);
convexpars = convexHullBinaryImage->GetOrderedParticleAnalysisReports();
//imaqGetParticleInfo()
//convexpars = convexHull->GetOrderedParticleAnalysisReports();
for (int i=0;i < convexHullBinaryImage->GetNumberParticles();i++)
{
//par = (*pars)[0];
//convexpar = (*convexpars)[i];
convexpar = convexHullBinaryImage->GetParticleAnalysisReport(i);
par = tapePixels->GetParticleAnalysisReport(i);
if((convexpar.boundingRect.width < 10) || (convexpar.boundingRect.height < 7))
{
continue;
}
// printf("%d par:%f convex:%f particle area\n",i,par.particleArea,convexpar.particleArea);
if ((par.particleArea/convexpar.particleArea > 0.4))
{
printf("%d skip max fillness ratio\n",i);
continue;
}
if ((par.particleArea/convexpar.particleArea < 0.10))
{
printf("%d skip min fillness ratio\n",i);
continue;
}
if((double)(convexpar.boundingRect.width)/(double)(convexpar.boundingRect.height)>1.8)
{
printf("%d skip max aspect ratio\n",i);
continue;
}
if((double)(convexpar.boundingRect.width)/(double)(convexpar.boundingRect.height)<.8)
{
printf("%d skip min aspect ratio\n",i);
continue;
}
//printf("%f center of mass x\n",par.center_mass_x_normalized);
//printf("%f center of mass y\n",par.center_mass_y_normalized);
distanceInInches = (18.0*179.3)/(convexpar.boundingRect.height);
double pwidth = convexpar.boundingRect.width;
double mwidth = ((double)convexpar.boundingRect.left+(double)convexpar.boundingRect.width*0.5);
double angle = ((180.0/3.14159)*acos (pwidth * distanceInInches/179.3/24.0) );
if(angle != angle) angle = 0.0; // if angle is NaN, set to zero
printf("%f distance in inches\n",distanceInInches);
//printf("%f angle\n",(180.0/3.14159)*acos (pwidth * distanceInInches/415.0/24.0) );
printf("%d BBctrX:%f CMX:%f\n", i, (double)convexpar.boundingRect.left + (double)convexpar.boundingRect.width*0.5, (double)par.center_mass_x);
//printf("%f angle2\n",(((pwidth * distanceInInches)/415.0)/24.0));
//printf("%f center of mass x\n",par.center_mass_x_normalized);
printf("%d %f %f center of mass x\n",i,convexpar.center_mass_x_normalized,par.center_mass_x_normalized);
printf("%d %f %f center of mass y\n",i,convexpar.center_mass_y_normalized,par.center_mass_y_normalized);
printf("%d %f %f rectangle score\n",i,(convexpar.particleArea)/((convexpar.boundingRect.width)*(convexpar.boundingRect.height))*(100),(par.particleArea)/((par.boundingRect.width)*(par.boundingRect.height))*(100));
printf("%d %f fillness ratio\n",i,par.particleArea/convexpar.particleArea);
printf("%d %d %d width and height\n",i,(convexpar.boundingRect.width),(convexpar.boundingRect.height));
printf("%d %f aspect ratio\n",i,((convexpar.boundingRect.width)/(double)(convexpar.boundingRect.height)));
if ((double)(par.center_mass_x)>mwidth)
{
angle=angle*(-1.0);
}
printf("%f true angle\n",angle);
//Wait(1.0);
double aiming_target_offset = 0.0;
//aiming_target_offset = pwidth * angle * (-0.5 / 45.0); numbers are iffy -> NaN
double speed = trackingFeedbackFunction(mwidth + aiming_target_offset - 80.0);
printf("%f aiming_target_offset due to %f degree angle\n", aiming_target_offset, angle);
printf("%f x offset \n",mwidth + aiming_target_offset - 80.0);
printf("%f speed \n", speed);
foundAnything = true;
if (use_alternate_score == false){
particle_score = convexpar.center_mass_y;
}
else{
particle_score = 2.0*fabs((double)convexpar.center_mass_y - 60.0) + fabs((double)convexpar.center_mass_x - 80.0);
}
// keep track of the *lowest* score
if (best_score > particle_score)
{
best_score = particle_score;
best_speed = speed;
}
}
if(foundAnything == false) {
myRobot->TankDrive(0.0, 0.0);
}
else
{
myRobot->TankDrive(-best_speed,best_speed);
}
delete img;
delete tapePixels;
delete pars;
delete convexHullBinaryImage;
delete convexpars;
//imaqDispose(convexHull);
if (foundAnything && best_speed == 0.0){
return true;
}
else
{
return false;
}
}
void AxisRenderer::renderOrigin(QGLShaderProgram &program, const AxisCamera &camera)
{
qDebug()<<"[AxisRender] drawing origin lines";
initializeGLFunctions();
//data for the xline
const GLfloat xline[] = {
0.0f, 0.0f, 0.0f, 1.0f,
5.0f, 0.0f, 0.0f, 1.0f
};
//data for the yline
const GLfloat yline[] = {
0.0f, 0.0f, 0.0f, 1.0f,
0.0f, 5.0f, 0.0f, 1.0f
};
//data for the zline
const GLfloat zline[] = {
0.0f, 0.0f, 0.0f, 1.0f,
0.0f, 0.0f, 5.0f, 1.0f
};
//data for each line color
const GLfloat xcolor[] = { 1.0f, 0.0f, 0.0f, 1.0f};
const GLfloat ycolor[] = { 0.0f, 1.0f, 0.0f, 1.0f};
const GLfloat zcolor[] = { 1.0f, 0.0f, 1.0f, 1.0f};
//fill buffer for xline
QGLBuffer xbuff(QGLBuffer::VertexBuffer);
xbuff.create();
xbuff.bind();
xbuff.setUsagePattern(QGLBuffer::StaticDraw);
xbuff.allocate(xline, sizeof(xline));
//fill buffer for yline
QGLBuffer ybuff(QGLBuffer::VertexBuffer);
ybuff.create();
ybuff.bind();
ybuff.setUsagePattern(QGLBuffer::StaticDraw);
ybuff.allocate(yline, sizeof(yline));
//fill buffer for zline
QGLBuffer zbuff(QGLBuffer::VertexBuffer);
zbuff.create();
zbuff.bind();
zbuff.setUsagePattern(QGLBuffer::StaticDraw);
zbuff.allocate(zline, sizeof(zline));
program.bind();
//set the uniforms
GLuint colorLoc = program.uniformLocation("color");
Q_ASSERT(colorLoc != -1);
GLuint mvpLoc = program.uniformLocation("modelToCamera");
Q_ASSERT(mvpLoc != -1);
program.setUniformValueArray(mvpLoc, &(camera.getProjMatrix()), 1);
//bind then draw xline
xbuff.bind();
program.enableAttributeArray("vertex");
program.setAttributeBuffer("vertex", GL_FLOAT, 0, 4);
program.setUniformValueArray(colorLoc, xcolor, 1, 4);
glDrawArrays(GL_LINES, 0, 2);
//bind then draw yline
ybuff.bind();
program.setAttributeBuffer("vertex", GL_FLOAT, 0, 4);
program.setUniformValueArray(colorLoc, ycolor, 1, 4);
glDrawArrays(GL_LINES, 0, 2);
//bind then draw zline
zbuff.bind();
program.setAttributeBuffer("vertex", GL_FLOAT, 0, 4);
program.setUniformValueArray(colorLoc, zcolor, 1, 4);
glDrawArrays(GL_LINES, 0, 2);
//destroy the buffers(Probably should be created and stored for longer than one call but context issue)
xbuff.bind();
xbuff.destroy();
ybuff.bind();
ybuff.destroy();
zbuff.bind();
zbuff.destroy();
}
void AxisRenderer::drawGrid(int gridSize, QGLShaderProgram &program,const AxisCamera &camera)
{
qDebug()<< "[AxisRender] Drawing Grid";
int gridNumber = camera.getGridFactor() * 2; //double size of grid to ensure it covers entire view
float lineLimit = gridNumber * gridSize; //bounds of grid for this size and number
qDebug()<< "[AxisRender] lineLimit = " << lineLimit;
//general xline for grid
float lineX[] = {lineLimit, 0.0f, 0.0f, 1.0f,
-lineLimit, 0.0f, 0.0f, 1.0f};
//general yline for grid
float lineY[] = { 0.0f, lineLimit, 0.0f, 1.0f,
0.0f, -lineLimit, 0.0f, 1.0f};
//general zline for grid
float lineZ[] = {0.0f, 0.0f, lineLimit, 1.0f,
0.0f, 0.0f, -lineLimit, 1.0f};
//bind and fill xline buffer
program.bind();
QGLBuffer bufferX(QGLBuffer::VertexBuffer);
bufferX.setUsagePattern(QGLBuffer::StaticDraw);
bufferX.create();
bufferX.bind();
bufferX.allocate(lineX,sizeof(lineX));
//bind and fill yline buffer
QGLBuffer bufferY(QGLBuffer::VertexBuffer);
bufferY.setUsagePattern(QGLBuffer::StaticDraw);
bufferY.create();
bufferY.bind();
bufferY.allocate(lineY, sizeof(lineY));
//bind and fill zline buffer
QGLBuffer bufferZ(QGLBuffer::VertexBuffer);
bufferZ.setUsagePattern(QGLBuffer::StaticDraw);
bufferZ.create();
bufferZ.bind();
bufferZ.allocate(lineZ, sizeof(lineZ));
QMatrix4x4 mvp;
QMatrix4x4 vp = camera.getProjMatrix();
QMatrix4x4 m;
//find the offest based on camera position
QVector3D pos = camera.getPosistion();
int xoff = pos.x() / gridSize;
int yoff = pos.y() / gridSize;
int zoff = pos.z() / gridSize;
//set color uniform to grey
QVector4D color(0.3f, 0.3f, 0.3f, 1.0f);
program.setUniformValueArray("color", &color, 1);
//draw each line
program.enableAttributeArray("vertex");
for(int i = -gridNumber; i <= gridNumber; ++i){
//x lines only for XY and XZ Axis
if(camera.getLock() == XY || camera.getLock() == XZ)
{
bufferX.bind();
program.setAttributeBuffer("vertex", GL_FLOAT, 0, 4);
//use model matrix to translate each line with offset
m.setToIdentity();
m.translate(0.0f + (xoff * gridSize), (i + yoff) * gridSize, (i + zoff) * gridSize);
mvp = vp * m ;//set mvp up
//set mvp uniform then draw
program.setUniformValueArray("modelToCamera", &mvp, 1);
glDrawArrays(GL_LINES, 0, 2);
}
//y lines for XY and YZ axis only
if(camera.getLock() == XY || camera.getLock() == YZ)
{
bufferY.bind();
program.setAttributeBuffer("vertex", GL_FLOAT, 0, 4);
//use model matrix to translate each line with offset
m.setToIdentity();
m.translate((i + xoff) * gridSize, 0.0f + (yoff * gridSize) ,(i + zoff) * gridSize);
mvp = vp * m;
//set mvp uniform and draw
program.setUniformValueArray("modelToCamera", &mvp, 1);
glDrawArrays(GL_LINES, 0, 2);
}
//z lines
if(camera.getLock() == XZ || camera.getLock() == YZ)
{
bufferZ.bind();
program.setAttributeBuffer("vertex", GL_FLOAT, 0, 4);
//use model matrix to translate each line with offset
m.setToIdentity();
m.translate((i + xoff) * gridSize, (i + yoff) * gridSize ,0.0f + (zoff * gridSize));
mvp = vp * m;
//set mvp uniform then draw
program.setUniformValueArray("modelToCamera", &mvp, 1);
glDrawArrays(GL_LINES, 0, 2);
}
}
}
Vision_Out Run(Vision_In in)
{
Vision_Out out;
//read file in from disk. For this example to run you need to copy image.jpg from the SampleImages folder to the
//directory shown below using FTP or SFTP: http://wpilib.screenstepslive.com/s/4485/m/24166/l/282299-roborio-ftp
//imaqError = imaqReadFile(frame, "//home//lvuser//SampleImages//image.jpg", NULL, NULL);
imaqError = camera->GetImage(frame);
//Update threshold values from SmartDashboard. For performance reasons it is recommended to remove this after calibration is finished.
/*
RING_HUE_RANGE.minValue = SmartDashboard::GetNumber("Tote hue min", RING_HUE_RANGE.minValue);
RING_HUE_RANGE.maxValue = SmartDashboard::GetNumber("Tote hue max", RING_HUE_RANGE.maxValue);
RING_SAT_RANGE.minValue = SmartDashboard::GetNumber("Tote sat min", RING_SAT_RANGE.minValue);
RING_SAT_RANGE.maxValue = SmartDashboard::GetNumber("Tote sat max", RING_SAT_RANGE.maxValue);
RING_VAL_RANGE.minValue = SmartDashboard::GetNumber("Tote val min", RING_VAL_RANGE.minValue);
RING_VAL_RANGE.maxValue = SmartDashboard::GetNumber("Tote val max", RING_VAL_RANGE.maxValue);
*/
if(in.shouldProcess)
{
//Threshold the image looking for ring light color
imaqError = imaqColorThreshold(binaryFrame, frame, 255, IMAQ_HSV, &RING_HUE_RANGE, &RING_SAT_RANGE, &RING_VAL_RANGE);
//Send particle count to dashboard
int numParticles = 0;
imaqError = imaqCountParticles(binaryFrame, 1, &numParticles);
SmartDashboard::PutNumber("Masked particles", numParticles);
//Send masked image to dashboard to assist in tweaking mask.
SendToDashboard(binaryFrame, imaqError);
//filter out small particles
float areaMin = SmartDashboard::GetNumber("Area min %", AREA_MINIMUM);
criteria[0] = {IMAQ_MT_AREA_BY_IMAGE_AREA, areaMin, 100, false, false};
imaqError = imaqParticleFilter4(binaryFrame, binaryFrame, criteria, 1, &filterOptions, NULL, NULL);
//Send particle count after filtering to dashboard
imaqError = imaqCountParticles(binaryFrame, 1, &numParticles);
SmartDashboard::PutNumber("Filtered particles", numParticles);
if(numParticles > 0)
{
//Measure particles and sort by particle size
std::vector<ParticleReport> particles;
for(int particleIndex = 0; particleIndex < numParticles; particleIndex++)
{
ParticleReport par;
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_AREA_BY_IMAGE_AREA, &(par.PercentAreaToImageArea));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_AREA, &(par.Area));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_TOP, &(par.BoundingRectTop));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_LEFT, &(par.BoundingRectLeft));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_BOTTOM, &(par.BoundingRectBottom));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_RIGHT, &(par.BoundingRectRight));
particles.push_back(par);
}
sort(particles.begin(), particles.end(), CompareParticleSizes);
//This example only scores the largest particle. Extending to score all particles and choosing the desired one is left as an exercise
//for the reader. Note that this scores and reports information about a single particle (single L shaped target). To get accurate information
//about the location of the tote (not just the distance) you will need to correlate two adjacent targets in order to find the true center of the tote.
scores.Aspect = AspectScore(particles.at(0));
SmartDashboard::PutNumber("Aspect", scores.Aspect);
scores.Area = AreaScore(particles.at(0));
SmartDashboard::PutNumber("Area", scores.Area);
bool isTarget = scores.Area > SCORE_MIN && scores.Aspect > SCORE_MIN;
//Send distance and tote status to dashboard. The bounding rect, particularly the horizontal center (left - right) may be useful for rotating/driving towards a tote
SmartDashboard::PutBoolean("IsTarget", isTarget);
double distance = computeDistance(binaryFrame, particles.at(0));
SmartDashboard::PutNumber("Distance", computeDistance(binaryFrame, particles.at(0)));
out.returnDistanceToTote = distance;
out.returnIsTote = true;
return out;
}
else
{
out.returnIsTote = false;
SmartDashboard::PutBoolean("IsTarget", false);
}
}
return out;
}
