// RobotBuilder Version: 0.0.2
//
// 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 th future.
package org.usfirst.frc330.Beachbot2013Java;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Preferences;
import edu.wpi.first.wpilibj.command.AutoSpreadsheet;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.usfirst.frc330.Beachbot2013Java.commands.*;
import org.usfirst.frc330.Beachbot2013Java.subsystems.*;
/*
 * $Log: Robot.java,v $
 * Revision 1.22  2013-03-16 18:00:48  echan
 * added the readvisionfile to robot
 *
 * Revision 1.21  2013-03-15 02:50:16  echan
 * added cvs log comments
 *
 */
 
/**
 * 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 OI oi;
    public static AutoSpreadsheet auto;
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    public static Chassis chassis;
    public static FrisbeePickup frisbeePickup;
    public static ShooterHigh shooterHigh;
    public static ShooterLow shooterLow;
    public static Vision vision;
    public static Arm arm;
    public static Climber climber;
    public static LCD lCD;
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    private static boolean practicerobot;
    public static boolean isPracticerobot() {
        return practicerobot;
    }
    public void robotInit() {
        if (!Preferences.getInstance().containsKey("PracticeRobot"))
        {
            Preferences.getInstance().putBoolean("PracticeRobot", false);
            Preferences.getInstance().save();
        }
        practicerobot = Preferences.getInstance().getBoolean("PracticeRobot", false);
	RobotMap.init();
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
        chassis = new Chassis();
        frisbeePickup = new FrisbeePickup();
        shooterHigh = new ShooterHigh();
        shooterLow = new ShooterLow();
        vision = new Vision();
        arm = new Arm();
        climber = new Climber();
        lCD = new LCD();
    // 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 = new OI();
        
        //initialize AutoSpreadsheet. Each auto command must be added here
        auto = new AutoSpreadsheet();
        auto.readScripts();
//        System.out.println("After Auto Read Scripts");
        auto.addCommand(new TurnGyroAbs(0));
        auto.addCommand(new TurnGyroRel(0));
        auto.addCommand(new Wait(0));
        auto.addCommand(new ShiftLow());
        auto.addCommand(new ShiftHigh());
        auto.addCommand(new DriveEncoder(0));
        auto.addCommand(new DriveEncoderRel());
        auto.addCommand(new DriveEncoderGyro(0,0));
        auto.addCommand(new DriveEncoderGyroRel());
        auto.addCommand(new DriveTime());
        auto.addCommand(new DriveEncoderGyroRamp(0,0));
        auto.addCommand(new DriveEncoderGyroRampRel());
        auto.addCommand(new DriveWaypoint(0,0,0,0,false));
        auto.addCommand(new TurnGyroWaypoint());
        auto.addCommand(new ArmClimbing());
        auto.addCommand(new ArmHighShooting());
        auto.addCommand(new ArmLowShooting());
        auto.addCommand(new ArmLowPickup());
        auto.addCommand(new HoldArm());
        auto.addCommand(new LaunchFrisbee());
//        auto.addCommand(new MarsRock());
        auto.addCommand(new PickupDown());
        auto.addCommand(new PickupFrisbeesOff());
        auto.addCommand(new PickupFrisbeesOn());
        auto.addCommand(new PickupUp());
        auto.addCommand(new ShootLow());
        auto.addCommand(new ShootLowCommandGroup());
        auto.addCommand(new TurnCamera());
        auto.addCommand(new DriveWaypointBackward(0,0,0,0,true));
        auto.addCommand(new TurnGyroWaypointBackward());
        auto.addCommand(new AutoWait());
        auto.addCommand(new TurnCameraIterative());
        auto.addCommand(new setGyroComp());
        auto.addCommand(new FullSpeedShootLow());
        
//        SmartDashboard.putData(Scheduler.getInstance());
    }
    public void autonomousInit() {
        // schedule the autonomous command (example)
        chassis.resetPosition();
        autonomousCommand = auto.getSelected();
        if (autonomousCommand != null) autonomousCommand.start();
    }
    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        chassis.calcPeriodic();
        frisbeePickup.calcPeriodic();
        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();
    }
    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        chassis.calcPeriodic();
        frisbeePickup.calcPeriodic();
        Scheduler.getInstance().run();
    }
    /**
     * This function called periodically during test mode
     */
    public void testPeriodic() {
        LiveWindow.run();
    }
    
    public void disabledPeriodic()
    {
        chassis.calcPeriodic();
        Scheduler.getInstance().run();
    }
    
    public void disabledInit()
    {
//        System.out.println("In Disabled Init");
        auto.readScripts();
        Robot.vision.readVisionFile();
    }
}

@@ -1010,16 +1010,82 @@
 ava,v $%0A
+ * Revision 1.23  2013-03-17 20:26:34  jross%0A * count frisbees%0A *%0A
  * Revis
@@ -5321,32 +5321,77 @@
 eedShootLow());%0A
+        auto.addCommand(new StopShootLow());%0A
         %0A//     

add additional commands