void LHSVision::UpdateVision() //by Michael
{
if(mXbox->GetRawButton(XBOX::XBOX_A_BUTTON) == true) //Toggle on A button press
{
while(mXbox->GetRawButton(XBOX::XBOX_A_BUTTON) == true) //Prevents multi-press
{}
if(send == 1)
{
StopCamera(1); //Stop Cam 1
StartCamera(2); //Start Cam 2
printf("\nActivating Camera 2 - Raw Image Display\n\n");
send = 2;
}
else
{
StopCamera(2); //Stop Cam 2
StartCamera(1); //Start Cam 1
printf("\nActivating Camera 1 - Raw Image Display\n\n");
send = 1;
}
}
if(send == 1)
{
IMAQdxGrab(session, frame, true, NULL);
SendToDashboard(frame); //Send Cam 1
Wait(.1);
}
else
{
IMAQdxGrab(session2, frame2, true, NULL);
SendToDashboard(frame2); //Send Cam 2
Wait(.1);
}
}
void CAMERAFEEDS::updateCam() {
int imaqError;
imaqError = IMAQdxGrab(curCam, frame, true, NULL);
if(imaqError != IMAQdxErrorSuccess) {
DriverStation::ReportError("IMAQdxGrab error: " + std::to_string((long)imaqError) + "\n");
}
server->SetImage(frame);
}
void TeleopPeriodic()
{
imaqError = IMAQdxGrab(session, camImage, true, NULL);
imMask->setImage(camImage);
if(imMask->maskImage()){
CameraServer::GetInstance()->SetImage(imMask->getbinImage());
}else{
DriverStation::ReportError("IMAQdxGram error: " + std::to_string((long)imMask->getError()));
}
cont.StickDrive();
cont.ButtonControl();
cont.DashPlace();
//imaqFlip(camImage, camImage, FlipAxis::IMAQ_HORIZONTAL_AXIS);
/*for(double i = 0.1; i <= 0.5; i+=0.05){
tal->Set(i);
SmartDashboard::PutNumber("RPM", cont.shoot->ReadRPM(banner, cont.shoot->rpmTimerL, x));
Wait(0.1);
}
for(double i = 0.5; i >= 0.1; i-=0.05){
tal->Set(i);
SmartDashboard::PutNumber("RPM", cont.shoot->ReadRPM(banner, cont.shoot->rpmTimerL, x));
Wait(0.1);
}
while(speed <= 1.0){
tal->Set(speed);
SmartDashboard::PutNumber("RPM", cont.shoot->ReadRPM(banner, cont.shoot->rpmTimerL, x));
Wait(0.25);
speed += 0.05;
}
while(speed >= 0.0){
tal->Set(speed);
SmartDashboard::PutNumber("RPM", cont.shoot->ReadRPM(banner, cont.shoot->rpmTimerL, x));
Wait(0.25);
speed -= 0.05;
}
tal->Set(1.0);
SmartDashboard::PutNumber("RPM", cont.shoot->ReadRPM(banner, cont.shoot->rpmTimerL, x));*/
//SmartDashboard::PutNumber("encoder", tal->GetEncPosition());
//SmartDashboard::PutBoolean("Banner", banner->Get());
//SmartDashboard::PutBoolean("Banner2", banner2->Get());
Wait(0.001);
}
int sgl_grab(CameraSgl *camera)
{
ImageInfo info;
IMAQdxError error = 0;
CameraData *data = &camera->data;
int left = camera->rect_show.left;
int top = camera->rect_show.top;
if (sgl_is_opend(camera)) {
error = IMAQdxGrab (camera->session_id, data->image, TRUE, &data->buffer_num);
if (error) {
goto ERROR_MSG;
}
if (camera->attached_win != NULL) {
if (camera->visible) {
imaqDisplayImage (camera->data.image, camera->display_win_num, FALSE);
if (error) {
goto ERROR_MSG;
}
// 获取额外的信息
imaqGetImageInfo (data->image, &info);
imaqMoveWindow ( camera->display_win_num, MakePoint( left, top) );
imaqShowWindow (camera->display_win_num, TRUE);
// printf("grab ---- visible\n");
}
else {
imaqShowWindow (camera->display_win_num, FALSE);
// printf("grab ---- invisible\n");
}
}
}
return 0;
ERROR_MSG:
sgl_camera_message_error(error, camera->error_callback);
return -1;
}
void TeleopPeriodic()
{
Scheduler::GetInstance()->Run();
IMAQdxGrab(session, frame, true, NULL);
if(imaqError != IMAQdxErrorSuccess) {
DriverStation::ReportError("IMAQdxGrab error: " + std::to_string((long)imaqError) + "\n");
} else {
//imaqDrawShapeOnImage(frame, frame, { 10, 10, 100, 100 }, DrawMode::IMAQ_DRAW_VALUE, ShapeMode::IMAQ_SHAPE_RECT, 1.0f);
//imaqDrawShapeOnImage(frame, frame, { 300, 220, 340, 260 }, DrawMode::IMAQ_DRAW_VALUE, ShapeMode::IMAQ_SHAPE_RECT, 1.0f);
CameraServer::GetInstance()->SetImage(frame);
}
// stop image acquisition
}
void Robot::TeleopPeriodic() {
Scheduler::GetInstance()->Run();
SmartDashboard::PutNumber("Arm Pos:",Robot::intakeArm->GetPos());
SmartDashboard::PutNumber("POV:",oi->getDriveStick()->GetPOV(0));
SmartDashboard::PutBoolean("old:",oldCameraSel);
SmartDashboard::PutBoolean("sel:",CameraSel);
double time = 0;
if (oldCameraSel != CameraSel)
{
time = Timer::GetFPGATimestamp();
IMAQdxStopAcquisition(session);
IMAQdxUnconfigureAcquisition(session);
IMAQdxCloseCamera(session);
if (CameraSel)
imaqError = IMAQdxOpenCamera("cam2", IMAQdxCameraControlModeController, &session);
else
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");
}
IMAQdxStartAcquisition(session);
oldCameraSel = CameraSel;
time = time - Timer::GetFPGATimestamp();
SmartDashboard::PutNumber("Switch:",time);
}
// if (CameraSel)
// IMAQdxGrab(session2, frame, true, NULL);
// else
IMAQdxGrab(session, frame, true, NULL);
if(imaqError != IMAQdxErrorSuccess) {
DriverStation::ReportError("IMAQdxGrab error: " + std::to_string((long)imaqError) + "\n");
} else {
CameraServer::GetInstance()->SetImage(frame);
}
}
IMAQdxError Camera::GetFrame() {
imaqError = IMAQdxErrorSuccess;
// grab an image,
if (session != ULONG_MAX && frame != NULL) {
if (false && firstTime) {
printf("Dumping modes\n");
DumpModes();
printf("Dumping attrs\n");
DumpAttrs();
}
IMAQdxGrab(session, frame, false, NULL);
if (imaqError != IMAQdxErrorSuccess) {
printf("IMAQdxConfigureGrab error (%s): %x\n",
camInfo[camera].InterfaceName, imaqError);
} else if (firstTime) {
int xRes, yRes;
imaqGetImageSize(frame, &xRes, &yRes);
printf("Camera: first image grab is %d x %d frame=%x\n", xRes, yRes,
(unsigned int) frame);
}
firstTime = false;
}
return imaqError;
}
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);
}
}
void Camera::SendImage() {
IMAQdxGrab(session, frame, true, NULL);
CameraServer::GetInstance()->SetImage(frame);
}
