/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.stuy;


import edu.stuy.camera.CameraVision;
import edu.stuy.commands.Autonomous;
import edu.stuy.commands.CommandBase;
import edu.stuy.commands.ConveyorPushDown;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * 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 JoeBot extends IterativeRobot {

    Command autonomousCommand;

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        // instantiate the command used for the autonomous period
        // autonomousCommand = new ExampleCommand();

        if (!Devmode.DEV_MODE) {
            Compressor compressor = new Compressor(RobotMap.PRESSURE_SWITCH_CHANNEL, RobotMap.COMPRESSOR_RELAY_CHANNEL);
            compressor.start();
        }

        // Initialize all subsystems
        CommandBase.init();
        
        CameraVision.getInstance();
    }

    public void autonomousInit() {
        // schedule the autonomous command (example)
        autonomousCommand = new Autonomous();
        autonomousCommand.start();
    }

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

    public void teleopInit() {
		// 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();
        }

                // Note that OI starts a bunch of other commands
                // by attaching them to joystick buttons.  Check OI.java
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        //
        Scheduler.getInstance().run();
        
        CameraVision.getInstance().doCamera();
        
        // Debug box actions
        SmartDashboard.putDouble("Distance 0", CameraVision.getInstance().getDistance(0));
        SmartDashboard.putDouble("Distance 1", CameraVision.getInstance().getDistance(1));
        SmartDashboard.putDouble("Distance 2", CameraVision.getInstance().getDistance(2));
        SmartDashboard.putDouble("Distance 3", CameraVision.getInstance().getDistance(3));
        SmartDashboard.putDouble("Target center", CameraVision.getInstance().getTargetCenter());
        SmartDashboard.putDouble("Center of mass 0", CameraVision.getInstance().getCenterMass(0));
        SmartDashboard.putDouble("Center of mass 1", CameraVision.getInstance().getCenterMass(1));
        SmartDashboard.putDouble("Center of mass 2", CameraVision.getInstance().getCenterMass(2));
        SmartDashboard.putDouble("Center of mass 3", CameraVision.getInstance().getCenterMass(3));
    }
}

@@ -3376,111 +3376,11 @@
 .put
-Double(%22Target center%22, CameraVision.getInstance().getTargetCenter());%0A        SmartDashboard.putDouble
+Int
 (%22Ce
@@ -3460,38 +3460,35 @@
 artDashboard.put
-Double
+Int
 (%22Center of mass
@@ -3556,38 +3556,35 @@
 artDashboard.put
-Double
+Int
 (%22Center of mass
@@ -3660,22 +3660,19 @@
 oard.put
-Double
+Int
 (%22Center

Modify SmartDashboard put to use ints.