public ControlMethodSelector() {
controlMethod = new SendableChooser();
controlMethod.addDefault("Tank Control", tankControl);
controlMethod.addObject("Stick Control", stickControl);
controlMethod.addObject("Clayton Control", claytonControl);
SmartDashboard.putData("Control method", controlMethod);
}
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public void robotInit() {
chooser = new SendableChooser();
chooser.addDefault("Default Auto", defaultAuto);
chooser.addObject("My Auto", customAuto);
SmartDashboard.putData("Auto choices", chooser);
controlMethod = new SendableChooser();
controlMethod.addDefault("Tank Control", tankControl);
controlMethod.addObject("Stick Control", stickControl);
controlMethod.addObject("Clayton Control", claytonControl);
SmartDashboard.putData("Control method", controlMethod);
rightMotor = new CANTalon(RIGHT_INDEX);
rightSlave = new CANTalon(RIGHT_INDEX + 1);
rightSlave.changeControlMode(CANTalon.TalonControlMode.Follower);
rightSlave.set(RIGHT_INDEX);
rightMotor.setInverted(true);
leftMotor = new CANTalon(LEFT_INDEX);
leftSlave = new CANTalon(LEFT_INDEX + 1);
leftSlave.changeControlMode(CANTalon.TalonControlMode.Follower);
leftSlave.set(LEFT_INDEX);
intakeMotor = new CANTalon(INTAKE_INDEX);
// Arm motor is currently disabled
// armMotor = new CANTalon(ARM_INDEX);
compressor = new Compressor(0);
compressor.setClosedLoopControl(true);
intakePneumatic = new DoubleSolenoid(1, 2);
intakePneumatic.set(DoubleSolenoid.Value.kOff);
gamepad = new Gamepad(0);
}
/**
* 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
chassisrotate = new Chassisrotate();
vision = new Vision();
chassis = new Chassis();
elevator = new Elevator();
shooter = new Shooter();
pickupBelt = new PickupBelt();
pickupShaft = new PickupShaft();
climber = new Climber();
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
autoChooser = new SendableChooser();
autoChooser.addDefault("Autonomous Position 1", new AutonomousCommand1());
autoChooser.addObject("Autonomous Position 2", new AutonomousCommand2());
SmartDashboard.putData("Autonomous Mode Chooser", autoChooser);
// 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();
SmartDashboard.putData(Robot.chassis);
// instantiate the command used for the autonomous period
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=AUTONOMOUS
autonomousCommand = new AutonomousCommand1();
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=AUTONOMOUS
}
public void teleopInit() {
START_TIME = System.currentTimeMillis();
if (autonomousCommand != null && autonomousCommand.isRunning()) {
autonomousCommand.cancel();
}
if (driveChooser.getSelected() != null) {
((Command) driveChooser.getSelected()).start();
}
intakeArm.initDefaultCommand();
intakeWheel.initDefaultCommand();
liftClimb.initDefaultCommand();
climbPiston.initDefaultCommand();
}
/** This function is called periodically during operator control */
public void teleopPeriodic() {
Scheduler.getInstance().run();
Object color = ledChooser.getSelected();
if (color != null && color instanceof Command) {
funCommand = (Command) color;
funCommand.start();
}
Object lights = vlChooser.getSelected();
if (lights != null && lights instanceof Command) {
vlCommand = (Command) lights;
vlCommand.start();
}
log();
}
public void checkForChange() {
if (autoChooser.getSelected() instanceof MarsRock) {
CommandGroup cg = new CommandGroup();
cg.addSequential(new MarsRock());
currentCommandGroup = cg;
return;
}
if (!selectedAuto.equalsIgnoreCase((String) autoChooser.getSelected())) {
selectedAuto = (String) autoChooser.getSelected();
currentCommandGroup = buildScript(selectedAuto);
System.out.println("Selected Auto: " + selectedAuto);
}
}
public void readScripts() {
try {
reader = new BufferedReader(new FileReader(filename));
String line;
String scriptName = null;
int comma1, comma2;
while ((line = reader.readLine()) != null) {
// System.out.println(line);
if (line.toUpperCase().startsWith("SCRIPT_NAME,")) {
comma1 = line.indexOf(",");
comma2 = line.substring(comma1 + 1).indexOf(",") + comma1 + 1;
scriptName = line.substring(comma1 + 1, comma2).toUpperCase().trim();
System.out.println("Found Script: " + scriptName);
autoChooser.addObject(scriptName, scriptName);
} else if (line.toUpperCase().startsWith("END_OF_SPREAD_SHEET,")) {
// System.out.println("End of Spreadsheet");
break;
}
}
SmartDashboard.putData("Autonomous mode", autoChooser);
} catch (IOException ex) {
ex.printStackTrace();
} finally {
try {
reader.close();
} catch (IOException ex) {
ex.printStackTrace();
}
currentCommandGroup = null;
}
currentCommandGroup = null;
}
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public void robotInit() {
oi = new OI();
chooser = new SendableChooser();
chooser.addDefault("Default Auto", new ExampleCommand());
// chooser.addObject("My Auto", new MyAutoCommand());
SmartDashboard.putData("Auto mode", chooser);
}
public void autonomousInit() {
// schedule the autonomous command (example)
autonomousCommand = (Command) autoChooser.getSelected();
if (autonomousCommand != null) {
autonomousCommand.start();
}
}
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public void robotInit() {
woodDrive = new Drivetrain();
testShooter = new Shooter();
oi = new OI();
chooser = new SendableChooser();
chooser.addDefault("Default Auto", new WoodDrive());
// chooser.addObject("My Auto", new MyAutoCommand());
SmartDashboard.putData("Auto mode", chooser);
}
public AutoSpreadsheet(String filename, String commandPackage) {
this.filename = filename;
this.commandPackage = commandPackage;
selectedAuto = new String();
// System.out.println("begin of AutoSpreadsheet Constructor");
autoChooser = new SendableChooser();
// System.out.println(MarsRock.class.getName());
autoChooser.addDefault("Mars Rock", new MarsRock());
// System.out.println("end of AutoSpreadsheet Constructor");
}
public void autonomousInit() {
// schedule the autonomous command (example)
// if (autonomousCommand != null)
Object selection = autonomousChooser.getSelected();
if (selection != null && selection instanceof Command) {
autonomousCommand = (Command) selection;
// autonomousCommand.start();
} else {
System.out.println("No autonomous mode selected.");
}
}
/**
* 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 code to get the auto name from the text box below the Gyro
*
* <p>You can add additional auto modes by adding additional commands to the chooser code above
* (like the commented example) or additional comparisons to the switch structure below with
* additional strings & commands.
*/
public void autonomousInit() {
autonomousCommand = (Command) chooser.getSelected();
String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
switch (autoSelected) {
case "Auto1":
autonomousCommand = new autonomous1();
break;
}
// schedule the autonomous command (example)
if (autonomousCommand != null) autonomousCommand.start();
}
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public void robotInit() {
// SmartDashboard stuff
chooser = new SendableChooser();
chooser.addObject("Touch", touch);
chooser.addObject("Lowbar Cross", lowBarCross);
chooser.addObject("Rockwall Cross", rockWallCross);
chooser.addObject("Rough Terrain Cross", roughTerrainCross);
SmartDashboard.putData("Auto Choices", chooser);
// Initializes joysticks
joystickLeft = new Joystick(0);
joystickRight = new Joystick(1);
joystickShooting = new Joystick(2);
// Duel camera class
cameraFeeds = new CameraFeeds(joystickShooting);
// Sets up classes
tankDrive = new Drive(joystickLeft, joystickRight);
driveShift = new Shifting(joystickShooting);
shooter = new Shooter(joystickShooting);
arm = new Arm(joystickShooting);
}
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public void robotInit() {
oi = new OI();
chooser = new SendableChooser();
chooser.addDefault("Auto1RightLow", new Auto1RightLow()); /*-Default-*/
chooser.addObject("Auto1RightHigh", new Auto1RightHigh());
chooser.addObject("Auto2RightLow", new Auto2RightLow());
chooser.addObject("Auto3MidHigh", new Auto3MidHigh());
chooser.addObject("Auto4MidHigh", new Auto4MidHigh());
chooser.addObject("Auto5LeftLow", new Auto5LeftLow());
chooser.addObject("Auto5MidHigh", new Auto5MidHigh());
chooser.addObject("AutoCross", new AutoCross());
chooser.addObject("AutoGenericRightLow", new AutoGenericRightLow());
chooser.addObject("AutoGenericLeftLow", new AutoGenericLeftLow());
// chooser.addObject("My Auto", new MyAutoCommand());
SmartDashboard.putData("Auto mode", chooser);
CameraServer.getInstance().startAutomaticCapture("cam2");
}
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public void robotInit() {
oi = new OI();
chooser = new SendableChooser();
chooser.addDefault("Default Auto", new ExampleCommand());
// chooser.addObject("My Auto", new MyAutoCommand());
SmartDashboard.putData("Auto mode", chooser);
try {
/* Communicate w/navX-MXP via the MXP SPI Bus. */
/* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */
/* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */
ahrs = new AHRS(SPI.Port.kMXP);
} catch (RuntimeException ex) {
DriverStation.reportError("Error instantiating navX-MXP: " + ex.getMessage(), true);
}
}
/**
* 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 code to get the auto name from the text box below the Gyro
*
* <p>You can add additional auto modes by adding additional commands to the chooser code above
* (like the commented example) or additional comparisons to the switch structure below with
* additional strings and commands.
*/
public void autonomousInit() {
autonomousCommand = (Command) chooser.getSelected();
/* String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
switch(autoSelected) {
case "My Auto":
autonomousCommand = new MyAutoCommand();
break;
case "Default Auto":
default:
autonomousCommand = new ExampleCommand();
break;
} */
// schedule the autonomous command (example)
if (autonomousCommand != null) autonomousCommand.start();
}
public void setupAutonomous() {
autoChooser = new SendableChooser();
// autoChooser.addDefault("Default: Drive Into Auto Zone", new AutoCGDriveStraightOneSecond());
autoChooser.addObject("Strafe Right Into Auto Zone", new AutoCGStrafeRight());
autoChooser.addObject("Strafe Left Into Auto Zone", new AutoCGStrafeLeft());
autoChooser.addDefault("Push Tote Forward", new AutoCGToteSetDriveForwardHook());
autoChooser.addObject("Lowers Arm - Does NOT move", new AutoCGDoNothing());
autoChooser.addObject("Picks Up Container And Drives Backwards", new AutoCGContainerPickup());
autoChooser.addObject("Picks Up Container And Does Not Drive", new AutoCGPickUpContainer());
SmartDashboard.putData("Autonomous Mode Chooser", autoChooser);
}
public void robotInit() {
// chooser = new SendableChooser();
com = new Compressor(RobotMap.PCM_ID);
oi = new OI();
drive = new DriveSubsystem();
intakeArm = new IntakeArmSubsystem();
intakeWheel = new IntakeWheelSubsystem();
climbPiston = new ClimbPistonSubsystem();
liftClimb = new LiftClimbSubsystem();
pushup = new PushupSubsystem();
oi.initButtonCommands();
driveChooser = new SendableChooser();
driveChooser.addDefault("Tedious Tank", new TankDriveCommand());
SmartDashboard.putData("Drive Chooser", driveChooser);
// chooser.addDefault("No operation autonomous", new WaitCommand(1));
// chooser.addObject("Main Autonomous", new TimedAuto());
// chooser.addObject("High Speed Auto", new HighSpeedAuto());
// chooser.addObject("Sandwhich!", new SandwichCommand());
// SmartDashboard.putData("Autonomous chooser", chooser);
// test.start();
}
private Cross getCross() {
return (Cross) cross.getSelected();
}
// * This function is run when the robot is first started up and should be
// * used for any initialization code.
// *
public void robotInit() {
// Initialize all subsystems.
pdp = new PDP();
drivetrain = new Drivetrain();
elevator = new Elevator();
toteContact = new ToteContactSensor();
lights = new FunLights();
vl = new VisionLights();
camera = new Camera();
new Thread(camera).start();
oi = new OI();
autonomousChooser.addDefault("No Autonomous", new AutonomousNone());
autonomousChooser.addObject("Left Yellow Tote", new AutonomousLeft());
autonomousChooser.addObject("Pull Bins Down", new AutoDriveNoLift());
SmartDashboard.putData("Autonomous Mode", autonomousChooser);
ledChooser.addObject("Off", new SetLEDColors(0));
ledChooser.addObject("White", new SetLEDColors(1));
ledChooser.addObject("Red", new SetLEDColors(2));
ledChooser.addObject("Blue", new SetLEDColors(3));
ledChooser.addDefault("Green", new SetLEDColors(4));
ledChooser.addObject("Yellow", new SetLEDColors(5));
ledChooser.addObject("Cyan", new SetLEDColors(6));
ledChooser.addObject("Magenta", new SetLEDColors(7));
ledChooser.addObject("Elevator", null);
SmartDashboard.putData("LED Color", ledChooser);
vlChooser.addDefault("Cam LEDs Off", new SetVisionLights(0));
vlChooser.addObject("Cam LEDs On", new SetVisionLights(1));
SmartDashboard.putData("Vision Lights", vlChooser);
// Show what command the subsystem is running on the SmartDashboard.
SmartDashboard.putData(drivetrain);
SmartDashboard.putData(elevator);
SmartDashboard.putData(toteContact);
SmartDashboard.putData(lights);
}
private Post getPost() {
return (Post) post.getSelected();
}
public IDrivetrainJoystick getControlMethod() {
return (IDrivetrainJoystick) controlMethod.getSelected();
}
private Cross getChevalReturn() {
return (Cross) chevalReturn.getSelected();
}
private IRIAutonChooser() {
start = new SendableChooser();
start.addObject("Low Bar (1)", Start.POS_1);
start.addObject("Position 2", Start.POS_2);
start.addDefault("Position 3", Start.POS_3);
start.addObject("Position 4", Start.POS_4);
start.addObject("Secret Passage (5)", Start.POS_5);
cross = new SendableChooser();
cross.addObject("A: Cheval De Frise (Left Return)", Cross.CHEVAL_DE_FRISE_A);
cross.addObject("A: Cheval De Frise (Right Return)", Cross.CHEVAL_DE_FRISE_B);
cross.addObject("B: Moat", Cross.MOAT);
cross.addObject("B: Ramparts", Cross.RAMPARTS);
cross.addObject("D: Rock Wall", Cross.ROCK_WALL);
cross.addDefault("D: Rough Terrain", Cross.ROUGH_TERRAIN);
cross.addObject("Low Bar", Cross.LOW_BAR);
chevalReturn = new SendableChooser();
chevalReturn.addObject("Low Bar", Cross.LOW_BAR);
chevalReturn.addObject("B: Moat", Cross.MOAT);
chevalReturn.addObject("B: Ramparts", Cross.RAMPARTS);
chevalReturn.addObject("C: Sally Port", Cross.SALLY_PORT);
chevalReturn.addObject("C: Drawbridge", Cross.DRAWBRIDGE);
chevalReturn.addObject("D: Rock Wall", Cross.ROCK_WALL);
chevalReturn.addDefault("D: Rough Terrain", Cross.ROUGH_TERRAIN);
post = new SendableChooser();
post.addDefault("Straighten", Post.STRAIGHTEN);
post.addObject("Go Back", Post.GO_BACK);
post.addObject("Attempt Two Ball", Post.TWO_BALL);
}
private Start getStart() {
return (Start) start.getSelected();
}
public CommandGroup getSelected() {
checkForChange();
if (currentCommandGroup != null) return currentCommandGroup;
else return buildScript((String) autoChooser.getSelected());
}
/**
* 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
*
* <p>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.
*/
public void autonomousInit() {
autoSelected = (String) chooser.getSelected();
// autoSelected = SmartDashboard.getString("Auto Selector", defaultAuto);
System.out.println("Auto selected: " + autoSelected);
}
/**
* 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
*
* <p>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.
*/
public void autonomousInit() {
autoSelected = (String) chooser.getSelected();
// autoSelected = SmartDashboard.getString("Auto Selector", defaultAuto);
System.out.println("Auto selected: " + autoSelected);
switch (autoSelected) {
case touch:
Timer.delay(0.5); // Wait 0.5 sec
// shooter.armAuto(0.50); // Lower arm - run motors at 50% speed for 0.5 sec
// Timer.delay(0.5);
// shooter.armAuto(0);
// Timer.delay(1.5);
tankDrive.auto(0.53, 0.60); // Move forward - run motors at 60% speed for 2 sec
Timer.delay(2);
tankDrive.auto(0, 0); // Stop robot
break;
case lowBarCross:
Timer.delay(0.5); // Wait 0.5 sec
// shooter.armAuto(0.50); // Lower arm - run motors at 50% speed for 0.5 sec
// Timer.delay(0.5);
// shooter.armAuto(0);
// Timer.delay(1.5);
tankDrive.auto(0.0, 0.60); // Move forward - run motors at 80% speed for 4 sec
Timer.delay(.1);
tankDrive.auto(0.53, 0.60); // Move forward - run motors at 60% speed for 4.5 sec
Timer.delay(4.5);
tankDrive.auto(0, 0); // Stop robot
break;
case rockWallCross:
Timer.delay(0.5); // Wait 0.5 sec
// shooter.armAuto(0.50); // Lower arm - run motors at 50% speed for 0.5 sec
// Timer.delay(0.5);
// shooter.armAuto(0);
// Timer.delay(1.5);
tankDrive.auto(0.0, 0.85); // Move forward - run motors at 80% speed for 4 sec
Timer.delay(.1);
tankDrive.auto(0.78, 0.85); // Move forward - run motors at 80% speed for 4 sec
Timer.delay(4.5);
tankDrive.auto(0, 0); // Stop robot
break;
case roughTerrainCross:
Timer.delay(0.5); // Wait 0.5 sec
// shooter.armAuto(0.50); // Lower arm - run motors at 50% speed for 0.5 sec
// Timer.delay(0.5);
// shooter.armAuto(0);
// Timer.delay(1.5);
tankDrive.auto(0.0, 0.80); // Move forward - run motors at 80% speed for 4 sec
Timer.delay(.1);
tankDrive.auto(0.73, 0.80); // Move forward - run motors at 80% speed for 4 sec
Timer.delay(4);
tankDrive.auto(0, 0); // Stop robot
break;
default:
// Nothing ...
break;
}
}
public void teleopInit() {
drivetrainJoystick = (IDrivetrainJoystick) controlMethod.getSelected();
// autoSelected = SmartDashboard.getString("Auto Selector", defaultAuto);
// System.out.println("Control method selected selected: " + controlSelected);
}
