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;
}
void CAMERAFEEDS::changeCam(int newId) {
int imaqError;
IMAQdxStopAcquisition(curCam);
imaqError = IMAQdxConfigureGrab(newId);
if(imaqError != IMAQdxErrorSuccess) {
DriverStation::ReportError("IMAQdxConfigureGrab error: " + std::to_string((long)imaqError) + "\n");
}
IMAQdxStartAcquisition(newId);
curCam = newId;
}
//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);
}
}
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);
}
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 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::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.
}
