Robot.java

// RobotBuilder Version: 2.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.


package org.usfirst.frc1982.Robot2014new;

import edu.wpi.first.wpilibj.ADXL362;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.DigitalOutput;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.SPI;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.interfaces.Accelerometer;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import java.nio.ByteBuffer;

import org.usfirst.frc1982.Robot2014new.commands.*;
import org.usfirst.frc1982.Robot2014new.subsystems.*;

import com.ni.vision.NIVision;
import com.ni.vision.NIVision.Image;

import edu.wpi.first.wpilibj.vision.USBCamera;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Robot extends IterativeRobot {

    Command autonomousCommand;
    
//    public static ADXL362 accel;
    public static ADXRS450_Gyro gyro;
    
    public static DigitalOutput led0;
    
    public static I2C ColorSensor;
    
    public static byte[] byteAr;
    public static ByteBuffer bytebuffer;
    
    public static int currSession;
    public static int sessionfront;
    public static int sessionback;
    public static Image frame;
    
    public static CameraServer server;
    
    public static boolean reversed = false;
    
//    public static CameraFeeds cameraFeeds = new CameraFeeds();

    public static OI oi;
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    public static Drive drive;
    public static Pneumatics pneumatics;
    public static Lift lift;

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    
//    CameraServerTwo server;

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
    RobotMap.init();
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
        drive = new Drive();
        pneumatics = new Pneumatics();
        lift = new Lift();

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
        // OI must be constructed after subsystems. If the OI creates Commands
        //(which it very likely will), subsystems are not guaranteed to be
        // constructed yet. Thus, their requires() statements may grab null
        // pointers. Bad news. Don't move it.
        oi = new OI();
        
        frame = NIVision.imaqCreateImage(NIVision.ImageType.IMAGE_RGB, 0);

        sessionfront = NIVision.IMAQdxOpenCamera("cam0", NIVision.IMAQdxCameraControlMode.CameraControlModeController);
                
        sessionback = NIVision.IMAQdxOpenCamera("cam1", NIVision.IMAQdxCameraControlMode.CameraControlModeController);

        currSession = sessionfront;

        NIVision.IMAQdxConfigureGrab(currSession);
        
        server = CameraServer.getInstance();
        
        server.setQuality(30);
        server.setSize(2);
        
//        accel = new ADXL362(SPI.Port.kMXP,Accelerometer.Range.k2G);
        gyro = new ADXRS450_Gyro(SPI.Port.kOnboardCS0);
        
        ColorSensor = new I2C(I2C.Port.kOnboard, 0x1E);
//        ColorSensor.write(0x03, 0);
        
        led0 = new DigitalOutput(0);
        led0.set(true);
        
        
        gyro.calibrate();
//        server = CameraServerTwo.getInstance();
//        server.setQuality(30);
//        
//        server.setSize(0);
//        
//        server.SetupCam("cam0");
//        server.SetupCam("cam1");
        
//        server.SwitchTo("cam1");

        // instantiate the command used for the autonomous period
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=AUTONOMOUS

        autonomousCommand = new AutonomousCommand();

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=AUTONOMOUS
    }

    /**
     * This function is called when the disabled button is hit.
     * You can use it to reset subsystems before shutting down.
     */
    public void disabledInit(){
//    	cameraFeeds.end();
    	gyro.calibrate();
    }

    public void disabledPeriodic() {
        Scheduler.getInstance().run();
        NIVision.IMAQdxGrab(currSession, frame, 1);
        server.setImage(frame);
        SmartDashboard.putNumber("Angle:", gyro.getAngle());
        SmartDashboard.putNumber("Rate: ", gyro.getRate());
        byteAr = new byte[1];
//        bytebuffer = ByteBuffer.allocateDirect(1);
        boolean success = !ColorSensor.read(0x04,1,byteAr);

//        System.out.println("Succes = " + success);
        if (success) {
        	System.out.println("color sensor returned true");
//        	System.out.println(byteAr[0]);
        } else {
        	System.out.println("color sensor returned false");
        }
        ColorSensor.write(0x03, 0x00);
    }

    public void autonomousInit() {
        // schedule the autonomous command (example)
        if (autonomousCommand != null) autonomousCommand.start();
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        Scheduler.getInstance().run();
    }

    public void teleopInit() {
//    	cameraFeeds.init();
        // This makes sure that the autonomous stops running when
        // teleop starts running. If you want the autonomous to
        // continue until interrupted by another command, remove
        // this line or comment it out.
        if (autonomousCommand != null) autonomousCommand.cancel();
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        Scheduler.getInstance().run();
//        cameraFeeds.updateCam();
//        System.out.println("X = " + accel.getX());
//        System.out.println("Y = " + accel.getY());
//        System.out.println("Z = " + accel.getZ());
//        System.out.println(gyro.getAngle());
        SmartDashboard.putNumber("Angle:", gyro.getAngle());
        SmartDashboard.putNumber("Rate: ", gyro.getRate());
        NIVision.IMAQdxGrab(currSession, frame, 1);
        server.setImage(frame);
    }

    /**
     * This function is called periodically during test mode
     */
    public void testPeriodic() {
        LiveWindow.run();
    }
}