void Robot::RobotInit() {
RobotMap::init();
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
chassis.reset(new Chassis());
intakeArm.reset(new IntakeArm());
rollers.reset(new Rollers());
lift.reset(new Lift());
liftAngle.reset(new LiftAngle());
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
// This MUST be here. If the OI creates Commands (which it very likely
// will), constructing it during the construction of CommandBase (from
// which commands extend), subsystems are not guaranteed to be
// yet. Thus, their requires() statements may grab null pointers. Bad
// news. Don't move it.
oi.reset(new OI());
// instantiate the command used for the autonomous period
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=AUTONOMOUS
//autonomousCommand.reset(new AutonomousCommand());
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=AUTONOMOUS
frame = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
//the camera name (ex "cam0") can be found through the roborio web interface
imaqError = IMAQdxOpenCamera("cam0", IMAQdxCameraControlModeController, &session);
if(imaqError != IMAQdxErrorSuccess) {
DriverStation::ReportError("IMAQdxOpenCamera error: " + std::to_string((long)imaqError) + "\n");
}
imaqError = IMAQdxConfigureGrab(session);
if(imaqError != IMAQdxErrorSuccess) {
DriverStation::ReportError("IMAQdxConfigureGrab error: " + std::to_string((long)imaqError) + "\n");
}
// imaqError = IMAQdxOpenCamera("cam2", IMAQdxCameraControlModeController, &session2);
// if(imaqError != IMAQdxErrorSuccess) {
// DriverStation::ReportError("IMAQdxOpenCamera error: " + std::to_string((long)imaqError) + "\n");
// }
// imaqError = IMAQdxConfigureGrab(session2);
// if(imaqError != IMAQdxErrorSuccess) {
// DriverStation::ReportError("IMAQdxConfigureGrab error: " + std::to_string((long)imaqError) + "\n");
// }
Robot::CameraSel = false;
Robot::oldCameraSel = false;
Robot::liftMode = false;
chooser = new SendableChooser();
chooser->AddDefault("Low Bar", new AutonomousCommand(0));
chooser->AddObject("Moat", new AutonomousCommand(1));
chooser->AddObject("Rock Wall", new AutonomousCommand(2));
chooser->AddObject("Nothing", new AutonomousCommand(42));
SmartDashboard::PutData("Autonomous Modes", chooser);
try{
ahrs = new AHRS(SPI::Port::kMXP);
} catch (std::exception ex) {
std::string err_string = "Error instantiating navX-MXP: ";
err_string += ex.what();
DriverStation::ReportError(err_string.c_str());
}
if (ahrs){
}
//Add another SendableChooser thing to allow robot to run and output ball during auto if needed.
}
void OperatorControl(void)
{
myRobot.SetSafetyEnabled(false);
AxisCamera &camera = AxisCamera::GetInstance("10.28.53.11");
camera.WriteResolution(AxisCamera::kResolution_320x240);
camera.WriteRotation(AxisCamera::kRotation_180);//Flip image upside-down.
camera.WriteCompression(80);//Compresses the image(?)
camera.WriteBrightness(50);//Sets the brightness to 80 on a scale from 0-100
DriverStationLCD *screen = DriverStationLCD::GetInstance();
int count = 0;
while(IsOperatorControl())
{
screen->UpdateLCD();
count++;
HSLImage* imgpointer; //declares a new hue saturation lum image
imgpointer = camera.GetImage(); //grabs an image to initialize that image
//imaqCreateImage(IMAQ_IMAGE_U8,)
//imaqCast(NULL, imgpointer, IMAQ_IMAGE_U8, NULL, -1);
BinaryImage* binImg = NULL; //declares a new binary image
//ThresholdHSL changes our regular image into a binary image.
/*hueLow Low value for hue
hueHigh High value for hue
saturationLow Low value for saturation
saturationHigh High value for saturation
luminenceLow Low value for luminence
luminenceHigh High value for luminence
*/
binImg = imgpointer->ThresholdHSL(1, 255, 1, 255, 230, 255);
//Saves the image to a temp directory
binImg->Write("/tmp/thresh.jpg");
//Writes the binary image to disk
imaqWriteFile(binImg->GetImaqImage(), "/tmp/thresh2.jpg", NULL);
delete imgpointer;
Image* Convex = imaqCreateImage(IMAQ_IMAGE_U8, 0);
int returnvalue;
returnvalue = imaqConvexHull(Convex, binImg->GetImaqImage(), TRUE);
// imaqWriteFile(Convex, "/tmp/convex.jpg", NULL);
delete binImg;
// screen->PrintfLine(DriverStationLCD::kUser_Line4,"convex is %d",returnvalue);
screen->UpdateLCD();
float lookuptable[256];
lookuptable[0] = 0;
lookuptable[1] = 65535;
//Converts image to 16 bit, then back to 8 bit.
Image* cast = imaqCreateImage(IMAQ_IMAGE_U16, 0);
imaqCast(cast, Convex, IMAQ_IMAGE_U16, lookuptable, 0);
imaqDispose(Convex);
Image* bitcast = imaqCreateImage(IMAQ_IMAGE_U8, 0);
imaqCast(bitcast, cast, IMAQ_IMAGE_U8, NULL, 0);
imaqDispose(cast);
// screen->PrintfLine(DriverStationLCD::kUser_Line3,"det %d", imaqGetLastError());//Notifies the user
imaqWriteFile(bitcast, "/tmp/bitcast.jpg", NULL);
//Image* SuperSize = im
//imaqCreateImage(IMAQ_IMAGE_U8, 2240);
//int returnvalue2;
//returnvalue2 = imaqSizeFilter(SuperSize, Convex, TRUE, 1, IMAQ_KEEP_LARGE, NULL);
//screen->UpdateLCD();
//imaqDispose (Convex);
// ROI *roi;
// Rect rectangle;
// rectangle.top = 0;
// rectangle.left = 0;
// rectangle.width = 320;
// rectangle.height = 120;
// imaqAddRectContour(roi,rectangle);
static RectangleDescriptor rectangleDescriptor =
{
30, // minWidth (All values are estimated)
200, // maxWidth
20, // minHeight
200 // maxHeight
};
static CurveOptions curveOptions = //extraction mode specifies what the VI identifies curves in the image. curve options are all the
{
IMAQ_NORMAL_IMAGE, // extractionMode
75, // threshold
IMAQ_NORMAL, // filterSize
25, // minLength
15, // rowStepSize
15, // columnStepSize
10, // maxEndPointGap
FALSE, // onlyClosed
FALSE // subpixelAccuracy
};
static ShapeDetectionOptions shapeOptions =
{
IMAQ_GEOMETRIC_MATCH_ROTATION_INVARIANT, // mode
NULL, // angle ranges
0, // num angle ranges
{75, 125}, // scale range
300 // minMatchScore
};
int matches = 0;
//double highscore = 0;
//int highestindex = -1;
float difference = 0;
float time = 0;
float average = 0;
double y = 0;
//The big important line of code that does important stuff
RectangleMatch* recmatch = imaqDetectRectangles(bitcast, &rectangleDescriptor, &curveOptions, &shapeOptions, NULL, &matches);
// DashboardDataSender *dds;
// dds = new DashboardDataSender;
//screen->PrintfLine(DriverStationLCD::kUser_Line3,"det %d", imaqGetLastError());
//screen->PrintfLine(DriverStationLCD::kUser_Line4,"Matches: %d",matches);//Notifies the user
screen->PrintfLine(DriverStationLCD::kUser_Line1,"Matches: %i",matches);//Notifies the user
screen->UpdateLCD();
/*for(int i = 0; i < matches; i++)
{
//screen->PrintfLine((DriverStationLCD::Line)(i+1),"%i,%i,%i",recmatch[i].height,recmatch[i].width,recmatch[i].score);
if(recmatch[i].score > highscore)
{
highscore = recmatch[i].score;
highestindex = i;
}
screen->PrintfLine(DriverStationLCD::kUser_Line1,"score %i, i %i", recmatch[i].score, i);
screen->UpdateLCD();
}*/
average = (recmatch->corner[1].x + recmatch->corner[3].x)/2;
y = (472/(recmatch->height-4));
screen->PrintfLine(DriverStationLCD::kUser_Line5,"%f",average);
screen->PrintfLine(DriverStationLCD::kUser_Line3," %f",distance(recmatch->height));
//screen->PrintfLine(DriverStationLCD::kUser_Line5,"%f,%f",recmatch->height, y);
screen->PrintfLine(DriverStationLCD::kUser_Line6,"%f,%f",ceil(recmatch->corner[1].x),ceil(recmatch->corner[3].x));
screen->UpdateLCD();
//screen->PrintfLine(DriverStationLCD::kUser_Line6,"%d, %d, %d, %d",recmatch->corner[1].x,recmatch->corner[2].x,recmatch->corner[3].x,recmatch->corner[4].x);
//screen->PrintfLine(DriverStationLCD::kUser_Line5,"%d, %d",recmatch->height,recmatch->width);
if(average == 0)
{
screen->PrintfLine(DriverStationLCD::kUser_Line1,"Image Not Found");
}
else if(average < 145)
{
difference = (160 - average);
time = difference*0.0062;
vic.Set(0.1);
Wait(time);
vic.Set(0.0);
screen->PrintfLine(DriverStationLCD::kUser_Line1,"Less!");
screen->UpdateLCD();
}
else if (average > 175)
{
difference = (average - 160);
time = difference*0.0062;
vic.Set(-0.1);
Wait(time);
vic.Set(0.0);
screen->PrintfLine(DriverStationLCD::kUser_Line1,"More!");
screen->UpdateLCD();
}
else
{
vic.Set(0);
screen->PrintfLine(DriverStationLCD::kUser_Line1,"Perfection!");
screen->UpdateLCD();
}
screen->PrintfLine(DriverStationLCD::kUser_Line2,"Time: %f",time);
screen->PrintfLine(DriverStationLCD::kUser_Line4,"difference %f",difference);
// screen->PrintfLine(DriverStationLCD::kUser_Line2,"Score %i, Index %i", highscore, highestindex);
/* if(matches > 0)
{
int counter = 0;
while (counter < 20)
{
average = (recmatch[highestindex].corner[1].x + recmatch[highestindex].corner[2].x + recmatch[highestindex].corner[3].x +recmatch[highestindex].corner[4].x)/4;
average2 = (recmatch[highestindex].corner[1].y + recmatch[highestindex].corner[2].y + recmatch[highestindex].corner[3].y +recmatch[highestindex].corner[4].y)/4;
// screen->PrintfLine(DriverStationLCD::kUser_Line5,"Center = %f, %f",average, average2);
screen->UpdateLCD();
if (average > 170)
{
vic.Set(-0.1);
//screen->PrintfLine(DriverStationLCD::kUser_Line1,"Left %f",vic.Get());
//screen->UpdateLCD();
}
else if (average < 150)
{
vic.Set(0.1);
//screen->PrintfLine(DriverStationLCD::kUser_Line1,"Right %f",vic.Get());
//screen->UpdateLCD();
}
else
{
counter = 20;
vic.Set(0);
// screen->PrintfLine(DriverStationLCD::kUser_Line6,"Not Mallory! %f",vic.Get());
screen->UpdateLCD();
}
counter++a;
}
*/
// imaqWriteFile(cast, "/tmp/linedetect.jpg", NULL);
// }
/* else
{
vic.Set(0);
// screen->PrintfLine(DriverStationLCD::kUser_Line6,"Not Mallory! %f",vic.Get());
screen->UpdateLCD();
}*/
imaqDispose(bitcast);
imaqDispose(recmatch);
//imaqDispose(roi);
vic.Set(0);
}
}
void Camera::GetInfo() {
Image* binFrame;
binFrame = imaqCreateImage(IMAQ_IMAGE_U8, 0);
Range Hue = {hueMin, hueMax};
Range Sat = {satMin, satMax};
Range Val = {valMin, valMax};
ParticleFilterCriteria2 criteria[1];
//ParticleFilterOptions2 filterOptions = {0, 0, 1, 1};
criteria[0] = {IMAQ_MT_AREA, 0, (float) aireMin, false, true};
int nbParticles(0);
IMAQdxGrab(session, frame, true, NULL);
imaqScale(frame, frame, 2, 2, ScalingMode::IMAQ_SCALE_SMALLER, IMAQ_NO_RECT);
imaqColorThreshold(binFrame, frame, 255, IMAQ_HSV, &Hue, &Sat, &Val);
imaqMorphology(binFrame, binFrame, IMAQ_DILATE, NULL);
//imaqParticleFilter4(binFrame, binFrame, &criteria[0], 1, &filterOptions, NULL, &nbParticles);
imaqCountParticles(binFrame, 0, &nbParticles);
CameraServer::GetInstance()->SetImage(binFrame);
int indexMax(0);
double aireMax(0);
if(nbParticles > 0) {
for(int particleIndex = 0; particleIndex < nbParticles; particleIndex++){
double aire (0);
imaqMeasureParticle(binFrame, particleIndex, 0, IMAQ_MT_AREA, &aire);
if (aire > aireMax){
aireMax = aire;
indexMax = particleIndex;
}
}
double hypothenuse(0);
int hauteurImage(0);
int largeurImage(0);
double centreX(0);
double centreY(0);
double angleX(0);
double angleY(0);
double offset(0);
imaqMeasureParticle(binFrame, indexMax, 0, IMAQ_MT_CENTER_OF_MASS_X, ¢reX);
imaqMeasureParticle(binFrame, indexMax, 0, IMAQ_MT_CENTER_OF_MASS_Y, ¢reY);
imaqGetImageSize(binFrame, &largeurImage, &hauteurImage);
double dHauteurImage(hauteurImage);
double dLargeurImage(largeurImage);
//Normalisation
centreX = ((2 * centreX) / dLargeurImage) - 1;
centreY = ((-2 * centreY) / dHauteurImage) + 1;
angleX = atan(centreX * tan(FOV_X * acos(-1) / 180.0));
angleY = atan(centreY * tan(FOV_Y * acos(-1) / 180.0));
distance = (TARGET_HEIGHT - CAMERA_HEIGHT) / tan(CAMERA_ANGLE * acos(-1) / 180 + angleY);
hypothenuse = sqrt(pow(distance, 2) + pow(TARGET_HEIGHT - CAMERA_HEIGHT, 2));
offset = hypothenuse * tan(angleX);
ecart = offset - CAMERA_OFFSET;
SmartDashboard::PutNumber("Distance Cible", distance);
SmartDashboard::PutNumber("Angle Cible", ecart);
SmartDashboard::PutNumber("Aire Particule", aireMax);
SmartDashboard::PutNumber("Nombre Particules", nbParticles);
SmartDashboard::PutNumber("Hypothenuse", hypothenuse);
SmartDashboard::PutNumber("Largeur image", largeurImage);
}
}
/**
* @brief Create an image object
* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL, IMAQ_IMAGE_COMPLEX,
* IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL, IMAQ_IMAGE_RGB_U64
* The border size is defaulted to 3 so that convolutional algorithms work at the
* edges.
* When you are finished with the created image, dispose of it by calling
* frcDispose().
* To get extended error information, call GetLastError().
*
* @param type Type of image to create
* @return Image* On success, this function returns the created image. On
* failure, it returns nullptr.
*/
Image* frcCreateImage(ImageType type) {
return imaqCreateImage(type, DEFAULT_BORDER_SIZE);
}