From: Meryem Esa Date: Thu, 24 Mar 2016 19:52:34 +0000 (-0700) Subject: add code for switching b/w cameras X-Git-Url: http://challenge-bot.com/repos/?p=3501%2Fstronghold-2016;a=commitdiff_plain;h=refs%2Fheads%2Fharel%2Fcompetition-hotfixes add code for switching b/w cameras --- diff --git a/src/org/usfirst/frc/team3501/robot/CameraFeeds.java b/src/org/usfirst/frc/team3501/robot/CameraFeeds.java new file mode 100755 index 00000000..3221e836 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/CameraFeeds.java @@ -0,0 +1,76 @@ +package org.usfirst.frc.team3501.robot; + +import com.ni.vision.NIVision; +import com.ni.vision.NIVision.Image; + +import edu.wpi.first.wpilibj.CameraServer; + +public class CameraFeeds { + + // camera id's + private int camIntake; + private int camShooter; + private int currentCam; + + private Image frame; + private CameraServer server; + + public CameraFeeds() { + // get camera id's by giving camera name on roboRio web interface + camIntake = NIVision.IMAQdxOpenCamera(Constants.CameraFeeds.camNameIntake, + NIVision.IMAQdxCameraControlMode.CameraControlModeController); + camShooter = NIVision.IMAQdxOpenCamera(Constants.CameraFeeds.camNameIntake, + NIVision.IMAQdxCameraControlMode.CameraControlModeController); + + // img that will contain camera img + frame = NIVision.imaqCreateImage(NIVision.ImageType.IMAGE_RGB, 0); + + // server we will give the image to + server = CameraServer.getInstance(); + server.setQuality(Constants.CameraFeeds.imgQuality); + + } + + public void init() { + changeCam(camShooter); + } + + public void run() { + // change camera based on direction robot is moving in (whatever is front) + // (initial front is shooter) + if (currentCam != camIntake && Robot.driveTrain.isFlipped()) + changeCam(camIntake); + else if (currentCam != camShooter) { + changeCam(camShooter); + } + updateCam(); + } + + /*** + * Close camera stream + */ + public void end() { + NIVision.IMAQdxStopAcquisition(currentCam); + } + + /*** + * switch which camera your getting images from + * + * @param newId + * for camera + */ + public void changeCam(int newId) { + NIVision.IMAQdxStopAcquisition(currentCam); + NIVision.IMAQdxConfigureGrab(newId); + NIVision.IMAQdxStartAcquisition(newId); + currentCam = newId; + } + + /*** + * Get img from current camera and give it to server + */ + public void updateCam() { + NIVision.IMAQdxGrab(currentCam, frame, 1); + server.setImage(frame); + } +} diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java index d63f88d3..4a8c96e6 100644 --- a/src/org/usfirst/frc/team3501/robot/Constants.java +++ b/src/org/usfirst/frc/team3501/robot/Constants.java @@ -60,7 +60,8 @@ public class Constants { public final static int ENCODER_RIGHT_A = 3; public final static int ENCODER_RIGHT_B = 4; - public static final double INCHES_PER_PULSE = ((3.66 / 5.14) * 6 * Math.PI) / 256; + public static final double INCHES_PER_PULSE = ((3.66 / 5.14) * 6 * Math.PI) + / 256; public static double kp = 0.013, ki = 0.000015, kd = -0.002; public static double encoderTolerance = 8.0; @@ -130,6 +131,17 @@ public class Constants { public static final double WAIT_TIME = 1.0; } + public static class CameraFeeds { + + // TODO: what are cam names? + public static final String camNameIntake = ""; + public static final String camNameShooter = ""; + + // TODO: img quality? + public static final int imgQuality = 0; + + } + public enum Defense { PORTCULLIS, SALLY_PORT, ROUGH_TERRAIN, LOW_BAR, CHIVAL_DE_FRISE, DRAWBRIDGE, MOAT, ROCK_WALL, RAMPART, NONE; } diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index ee2e03da..be58f085 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -19,6 +19,7 @@ public class Robot extends IterativeRobot { public static Shooter shooter; public static IntakeArm intakeArm; public static Photogate photogate; + public static CameraFeeds cameraFeeds; // Sendable Choosers send a drop down menu to the Smart Dashboard. private static SendableChooser defenseChooser; @@ -31,6 +32,7 @@ public class Robot extends IterativeRobot { shooter = new Shooter(); intakeArm = new IntakeArm(); photogate = new Photogate(); + cameraFeeds = new CameraFeeds(); oi = new OI(); @@ -83,15 +85,18 @@ public class Robot extends IterativeRobot { @Override public void teleopInit() { // if intake is front, switch back to shooter as front + // this is done on purpose! if (choseIntakeIsFront) driveTrain.toggleFlipped(); + cameraFeeds.init(); Scheduler.getInstance().add(new SetLowGear()); } @Override public void teleopPeriodic() { Scheduler.getInstance().run(); + cameraFeeds.run(); } } diff --git a/src/org/usfirst/frc/team3501/robot/commands/intakearm/RunIntakeContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/intakearm/RunIntakeContinuous.java index 989cde69..d5cd3737 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/intakearm/RunIntakeContinuous.java +++ b/src/org/usfirst/frc/team3501/robot/commands/intakearm/RunIntakeContinuous.java @@ -25,8 +25,8 @@ public class RunIntakeContinuous extends Command { @Override protected void execute() { - if (Photogate.ballState()) - Robot.intakeArm.outputBall(); + // if (Photogate.ballState()) + Robot.intakeArm.outputBall(); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index 1079d501..807bb151 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -164,10 +164,10 @@ public class DriveTrain extends PIDSubsystem { /* * Method is a required method that the PID Subsystem uses to return the * calculated PID value to the driver - * + * * @param Gives the user the output from the PID algorithm that is calculated * internally - * + * * Body: Uses the output, does some filtering and drives the robot */ @Override @@ -190,7 +190,7 @@ public class DriveTrain extends PIDSubsystem { /* * Checks the drive mode - * + * * @return the current state of the robot in each state Average distance from * both sides of tank drive for Encoder Mode Angle from the gyro in GYRO_MODE */ @@ -207,8 +207,15 @@ public class DriveTrain extends PIDSubsystem { public void setMotorSpeeds(double left, double right) { double k = (isFlipped() ? -1 : 1); - cheese.cheesyDrive(-left * k, -right, - getGearPistonValue() == Constants.DriveTrain.HIGH_GEAR); + /* + * cheese.cheesyDrive(-left * k, -right, getGearPistonValue() == + * Constants.DriveTrain.HIGH_GEAR); + */ + + frontLeft.set(left); + rearLeft.set(left); + frontRight.set(right); + rearRight.set(right); } /** @@ -246,8 +253,8 @@ public class DriveTrain extends PIDSubsystem { */ public void switchGear() { Value currentValue = getGearPistonValue(); - Value setValue = (currentValue == Constants.DriveTrain.HIGH_GEAR) ? Constants.DriveTrain.LOW_GEAR - : Constants.DriveTrain.HIGH_GEAR; + Value setValue = (currentValue == Constants.DriveTrain.HIGH_GEAR) + ? Constants.DriveTrain.LOW_GEAR : Constants.DriveTrain.HIGH_GEAR; changeGear(setValue); }