void RobotInit()
{
camImage = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
binImage = imaqCreateImage(IMAQ_IMAGE_U8, 0);
imMask = new ImageMask(camImage, binImage, imaqError = 0);
chooser = new SendableChooser();
chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
SmartDashboard::PutData("Auto Modes", chooser);
imaqError = IMAQdxOpenCamera("cam1", IMAQdxCameraControlModeController, &session);
if(imaqError != IMAQdxErrorSuccess){
DriverStation::ReportError("IMAQdxOpencamera error: " + std::to_string((long)imaqError));
}
imaqError = IMAQdxConfigureGrab(session);
if(imaqError != IMAQdxErrorSuccess){
DriverStation::ReportError("IMAQdxConfigureGram error: " + std::to_string((long)imaqError));
}
/*
CameraServer::GetInstance()->SetQuality(50);
//the camera name (ex "cam0") can be found through the roborio web interface
CameraServer::GetInstance()->StartAutomaticCapture("cam1");
*/
//camImage = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
//tal = new CANTalon(1);
//banner = new DigitalInput(4);
//banner2 = new DigitalInput(5);
//tal->SetFeedbackDevice(CANTalon::CtreMagEncoder_Absolute);
//x = 0;
}
//CAMERAFEEDS::CAMERAFEEDS(Joystick *newJoy)
CAMERAFEEDS::CAMERAFEEDS(){
int imaqError;
imaqError = IMAQdxOpenCamera(camNameFront, IMAQdxCameraControlModeController, &camFront);
if(imaqError != IMAQdxErrorSuccess) {
DriverStation::ReportError("IMAQdxOpenCamera error: " + std::to_string((long)imaqError) + "\n");
}
imaqError = IMAQdxOpenCamera(camNameBack, IMAQdxCameraControlModeController, &camBack);
if(imaqError != IMAQdxErrorSuccess) {
DriverStation::ReportError("IMAQdxOpenCamera error: " + std::to_string((long)imaqError) + "\n");
}
curCam = camFront;
frame = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
server = CameraServer::GetInstance();
server->SetQuality(imgQuality);
//contrlr = newJoy;
}
//Used for opening, configuring, and starting acquisition of a camera
void LHSVision::StartCamera(int camNum)
{
if(camNum == 1) //Camera 1 Startup
{
IMAQdxOpenCamera(CAMERA::CAM_ONE, IMAQdxCameraControlModeController, &session);
Wait(.01);
IMAQdxConfigureGrab(session);
Wait(.01);
IMAQdxStartAcquisition(session);
Wait(.01);
}
else //Camera 2 Startup
{
IMAQdxOpenCamera(CAMERA::CAM_TWO, IMAQdxCameraControlModeController, &session2);
Wait(.01);
IMAQdxConfigureGrab(session2);
Wait(.01);
IMAQdxStartAcquisition(session2);
Wait(.01);
}
}
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);
}
}
Camera::Camera() :
Subsystem("Camera")
{
//cam = RobotMap::cameraCam;
IMAQdxOpenCamera("cam0", IMAQdxCameraControlModeController, &session);
//Wait(0.5);
IMAQdxConfigureGrab(session);
//Wait(0.5);
IMAQdxStartAcquisition(session);
//Wait(0.5);
frame = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
}
int sgl_open_camera(char *interface_name, CameraSgl *camera)
{
IMAQdxSession session_id;
IMAQdxError error = 0;
int display_win_num;
int i = 0;
error = IMAQdxOpenCamera (interface_name, IMAQdxCameraControlModeController, &session_id);
if (error) {
goto ERROR_MSG;
}
imaqGetWindowHandle (&display_win_num);
// OK，则返回正确的CameraSgl
camera->session_id = session_id;
camera->display_win_num = display_win_num;
camera->visible = TRUE;
size_t size = sizeof(camera->interface_name);
memset(camera->interface_name, 0, size);
strncpy(camera->interface_name, interface_name, size);
for (i = 0; i < camera->camera_count; i++) {
if (0 == strcmp(interface_name, camera->info.camera_info[i].InterfaceName)) {
camera->info.info_id = i;
}
}
// 获取相机的信息
IMAQdxGetAttribute(camera->session_id, IMAQdxAttributeWidth, IMAQdxValueTypeU32, &camera->info.width);
IMAQdxGetAttribute(camera->session_id, IMAQdxAttributeHeight, IMAQdxValueTypeU32, &camera->info.height);
IMAQdxGetAttribute(camera->session_id, IMAQdxAttributeBayerGainR, IMAQdxValueTypeF64, &camera->info.R);
IMAQdxGetAttribute(camera->session_id, IMAQdxAttributeBayerGainG, IMAQdxValueTypeF64, &camera->info.G);
IMAQdxGetAttribute(camera->session_id, IMAQdxAttributeBayerGainB, IMAQdxValueTypeF64, &camera->info.B);
sgl_init_camera_data(camera, IMAQ_IMAGE_RGB);
// 注册回调接口
//IMAQdxRegisterFrameDoneEvent (camera->session_id, 1, frame_done_callback, NULL);
sgl_set_acquisition_mode(camera, 1);
return 0;
ERROR_MSG:
sgl_camera_message_error(error, camera->error_callback);
return -1;
}
void RobotInit()
{
CommandBase::init();
frame = imaqCreateImage(IMAQ_IMAGE_HSL, 0);
imaqError = IMAQdxOpenCamera("cam1", 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");
}
testCommand = new TestCommand();
driveCommand = new DriveCommand();
}
//NOTE: Camera 1 is the default camera. Can toggle to 2 later.
LHSVision::LHSVision(RobotDrive* pRobot, Joystick* pXbox) //Constructor
{
printf("File %18s Date %s Time %s Object %p\n",__FILE__,__DATE__, __TIME__, this);
frame = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
IMAQdxOpenCamera(CAMERA::CAM_ONE, IMAQdxCameraControlModeController, &session);
Wait(.5);
IMAQdxConfigureGrab(session);
Wait(.5);
IMAQdxStartAcquisition(session);
Wait(.5);
frame2 = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
mRobot = pRobot;
mXbox = pXbox;
}
void USBCamera::OpenCamera() {
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
for (unsigned int i = 0; i < 3; i++) {
uInt32 id = 0;
// Can't use SAFE_IMAQ_CALL here because we only error on the third time
IMAQdxError error = IMAQdxOpenCamera(
m_name.c_str(), IMAQdxCameraControlModeController, &id);
if (error != IMAQdxErrorSuccess) {
// Only error on the 3rd try
if (i >= 2) wpi_setImaqErrorWithContext(error, "IMAQdxOpenCamera");
// Sleep for a few seconds to ensure the error has been dealt with
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
} else {
m_id = id;
m_open = true;
return;
}
}
}
IMAQdxError Camera::Start() {
// create an image
printf("Opening camera %s\n", camInfo[camera].InterfaceName);
imaqError = IMAQdxOpenCamera(camInfo[camera].InterfaceName,
IMAQdxCameraControlModeController, &session);
if (imaqError != IMAQdxErrorSuccess) {
printf("IMAQdxOpenCamera error: %x\n", imaqError);
session = ULONG_MAX;
return imaqError;
}
// imaqError = Camera::SetMode();
if (strstr(camInfo[camera].ModelName, "LifeCam") != nullptr)
SetVideoMode(416, 240, 15, false);
else if (strstr(camInfo[camera].ModelName, "Logitech Webcam") != nullptr)
SetVideoMode(640, 360, 24, false);
else {
if (!SetVideoMode(800, 600, 10, false)) // Genuis wide angle camera, low frame rate
SetVideoMode(320, 240, 15, false); // default to a widely supported mode
}
if (imaqError != IMAQdxErrorSuccess) {
return imaqError;
}
imaqError = IMAQdxConfigureGrab(session);
if (imaqError != IMAQdxErrorSuccess) {
printf("IMAQdxOpenCamera error: %x\n", imaqError);
return imaqError;
}
// start to acquire images
IMAQdxStartAcquisition(session);
if (imaqError != IMAQdxErrorSuccess) {
printf("IMAQdxOpenCamera error: %x\n", imaqError);
return imaqError;
}
frame = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
printf("Camera Opened for %s #: %lu, frame=%x\n",
camInfo[camera].InterfaceName, session, (unsigned int) frame);
return imaqError;
}
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.
}
