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 (executable)
index 0000000..3221e83
--- /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 d63f88d34c7e8f017fd43256d1c0408d566829ae..4a8c96e68b4649d71abe016e281cbaeee97e53fc 100644 (file)
--- 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 ee2e03da66e5fbf8ff3ab8926d64e57eaec685b8..be58f085bc576b4e902aa884a02877df0ce9611d 100644 (file)
--- 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 989cde690a646762181c5a18dca2409a54a5d3a4..d5cd37373588cae3f42415b5d7fdcb7e2c2df8c8 100644 (file)
--- 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 1079d501a82fc4f52d281b0c8e96478a2a0653a9..807bb151ec2d3b239329c3f9aafeabd41ff438f8 100644 (file)
--- 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);
   }