Drive Train Mk 2

package org.usfirst.frc.team4827.robot;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.Talon;

/**
 * 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 {

    RobotDrive myDrive;
    Joystick left, right;
    Joystick button;
    Victor lift=new Victor(5);
    Talon shooter=new Talon(6);
    Talon picker=new Talon(7);
    XboxController xbox;

    final String defaultAuto = "Default";
    final String customAuto = "My Auto";
    String autoSelected;
    SendableChooser<String> chooser = new SendableChooser<>();

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    @Override
    public void robotInit() {
        myDrive = new RobotDrive(0,1,2, 3);

        left = new Joystick(0);

        //XboxController xbox = new XboxController(1);
        right = new Joystick(1);


        chooser.addDefault("Default Auto", defaultAuto);
        chooser.addObject("My Auto", customAuto);
        SmartDashboard.putData("Auto choices", chooser);
    }

    /**
     * This autonomous (along with the chooser code above) shows how to select
     * between different autonomous modes using the dashboard. The sendable
     * chooser code works with the Java SmartDashboard. If you prefer the
     * LabVIEW Dashboard, remove all of the chooser code and uncomment the
     * getString line to get the auto name from the text box below the Gyro
     *
     * You can add additional auto modes by adding additional comparisons to the
     * switch structure below with additional strings. If using the
     * SendableChooser make sure to add them to the chooser code above as well.
     */
    @Override
    public void autonomousInit() {
        autoSelected = chooser.getSelected();
        // autoSelected = SmartDashboard.getString("Auto Selector",
        // defaultAuto);
        System.out.println("Auto selected: " + autoSelected);
    }

    /**
     * This function is called periodically during autonomous
     */
    @Override
    public void autonomousPeriodic() {
        switch (autoSelected) {
        case customAuto:
            // Put custom auto code here
            break;
        case defaultAuto:
        default:
            // Put default auto code here
            break;
        }
    }

    //public void operatorControl() {
        //while (isOperatorControl() && isEnabled()) {
            //myDrive.tankDrive(left, right);;
            //Timer.delay(0.01);
//      }
    //}

    /**
     * This function is called periodically during operator control
     */
    @Override
    public void teleopPeriodic() {
        while (isOperatorControl()&&isEnabled()){
            myDrive.tankDrive(left, right);//left.getAxis(Joystick.AxisType.kY), left.getAxis(Joystick.AxisType.kX));

                if(left.getRawButton(3)){
                    lift.set(0.5);
                }else{ lift.set(0.0);
                }

                if(left.getRawButton(1)){
                    picker.set(0.5);
                }else{ picker.set(0.0);

                //if(left.getRawAxis(3)){
                    //shooter.set(1.0);
                //}else{ shooter.set(0.0);

                }

                }
            }
        //}

    /**
     * This function is called periodically during test mode
     */
    @Override
    public void testPeriodic() {
    }
}
//}