Arizona Day 0: Teleoperated Functionality Mainly Completed

src/com/team3925/frc2018/OI.java

@@ -8,6 +8,8 @@
 import com.team3925.frc2018.commands.OpenGrabbers;
 import com.team3925.frc2018.commands.RunElevator;
 import com.team3925.frc2018.commands.RunIntakeWheels;
+import com.team3925.frc2018.commands.SetLiftBottom;
+import com.team3925.frc2018.commands.SetLiftTop;
 import com.team3925.frc2018.commands.ShiftHigh;
 import com.team3925.frc2018.commands.ShiftLow;
 import com.team3925.frc2018.commands.ShootCube;
@@ -62,8 +64,8 @@ private OI() {
 
 		drivetrain_Shift = new JoystickButton(wheel, 5);
 
-		openIntake = new JoystickButton(xbox, 6);
-		intakeCube = new JoystickButton(xbox, 5);
+		openIntake = new JoystickButton(xbox, 5);
+		intakeCube = new JoystickButton(xbox, 6);
 
 		dropCube = new Trigger() {
 			@Override
@@ -113,7 +115,9 @@ public boolean get() {
 		openIntake.whenReleased(new CloseGrabbers());
 
 		intakeCube.whenPressed(new RunIntakeWheels(1));
+		intakeCube.whenPressed(new SetLiftBottom());
 		intakeCube.whenReleased(new RunIntakeWheels(0));
+		intakeCube.whenReleased(new SetLiftTop());
 
 		tuneUp.whenActive(new IncrementAdjustElevator());
 		tuneDown.whenActive(new DecrementAdjustElevator());
@@ -137,7 +141,7 @@ public double getLiftIntake() {
 	}
 
 	public double getRawElevator() {
-		return stick.getRawAxis(5);
+		return xbox.getRawAxis(5);
 	}
 	public boolean getTestButton() {
 		return wheel.getRawButton(4);

src/com/team3925/frc2018/Robot.java

@@ -1,15 +1,12 @@
 package com.team3925.frc2018;
 
-import javax.swing.text.DefaultStyledDocument.ElementBuffer;
-
-import com.team3925.frc2018.commands.Center_Left_Switch_Auto;
 import com.team3925.frc2018.commands.DriveManual;
 import com.team3925.frc2018.commands.RunElevator;
 import com.team3925.frc2018.commands.RunElevatorRaw;
 import com.team3925.frc2018.commands.RunIntakeLiftRaw;
-import com.team3925.frc2018.commands.ShootCube;
+import com.team3925.frc2018.commands.ZeroIntake;
 import com.team3925.frc2018.subsystems.Elevator;
-import com.team3925.frc2018.subsystems.Elevator.ElevatorState;
+import com.team3925.frc2018.subsystems.Intake;
 
 import edu.wpi.first.wpilibj.IterativeRobot;
 import edu.wpi.first.wpilibj.command.Command;
@@ -21,28 +18,38 @@ public class Robot extends IterativeRobot {
 	RunElevatorRaw elevateRaw;
 	RunIntakeLiftRaw liftRaw;
 	Command testAuto;
+	Command zeroIntake;
 
 	@Override
 	public void robotInit() {
 		drive = new DriveManual(OI.getInstance());
 		elevateRaw = new RunElevatorRaw();
 		liftRaw = new RunIntakeLiftRaw();
-		testAuto = new Center_Left_Switch_Auto();
+		zeroIntake = new ZeroIntake();
 	}
 
 	@Override
 	public void autonomousInit() {
-		Elevator.getInstance().zero();
-		testAuto.start();
+//		Elevator.getInstance().zero();
+//		
+//		if (DriverStation.getInstance().getGameSpecificMessage().charAt(0) == 'L') {
+//			testAuto = new CenterLeftSwitchAuto();
+//		}else if(DriverStation.getInstance().getGameSpecificMessage().charAt(0) == 'R') {
+//			testAuto = new CenterRightSwitchAuto();
+//		}
+//		testAuto.start();
 	}
 
 	public void teleopInit() {
 		Elevator.getInstance().zero();
 		drive.start();
+		Intake.getInstance().setIntakeRollers(-0.1);
+		zeroIntake.start();
 	}
 
 	@Override
 	public void teleopPeriodic() {
+
 	}
 
 	@Override

src/com/team3925/frc2018/RobotMap.java

@@ -32,7 +32,7 @@ public static final class DrivetrainMap {
 
 	public static final class IntakeMap {
 		public static final WPI_TalonSRX LEFT_INTAKE = CTREControllerFactory.createDefaultTalon(11);
-		public static final WPI_VictorSPX RIGHT_INTAKE = CTREControllerFactory.createDefaultVictor(12);
+		public static final WPI_TalonSRX RIGHT_INTAKE = CTREControllerFactory.createDefaultTalon(12);
 
 		public static final WPI_TalonSRX LIFT_MOTOR = CTREControllerFactory.createDefaultTalon(13);
 

src/com/team3925/frc2018/commands/DropCube.java

@@ -8,7 +8,7 @@
 public class DropCube extends CommandGroup{
 	public DropCube() {
 		addParallel(new OpenGrabbers());
-		addSequential(new RunIntakeWheels(-0.25));
+		addSequential(new RunIntakeWheels(-0.40));
 		addSequential(new WaitCommand(0.5));
 		addParallel(new RunIntakeWheels(0));
 		addSequential(new CloseGrabbers());

src/com/team3925/frc2018/commands/RunElevator.java

@@ -2,6 +2,7 @@
 
 import com.team3925.frc2018.OI;
 import com.team3925.frc2018.subsystems.Elevator;
+import com.team3925.frc2018.subsystems.Intake;
 import com.team3925.frc2018.subsystems.Elevator.ElevatorState;
 
 import edu.wpi.first.wpilibj.command.Command;
@@ -24,4 +25,9 @@ protected void initialize() {
 	protected boolean isFinished() {
 		return true;
 	}
+
+	@Override
+	protected void end() {
+		Intake.getInstance().setAngle(0);
+	}
 }

src/com/team3925/frc2018/commands/SetLiftBottom.java

@@ -0,0 +1,46 @@
+package com.team3925.frc2018.commands;
+
+import com.team3925.frc2018.subsystems.Elevator;
+import com.team3925.frc2018.subsystems.Elevator.ElevatorState;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+public class SetLiftBottom extends CommandGroup {
+
+	public SetLiftBottom() {
+
+		addSequential(new Command() {
+
+			@Override
+			protected void initialize() {
+				Elevator.getInstance().setPosition(ElevatorState.BOTTOM);
+			}
+
+			@Override
+			protected boolean isFinished() {
+				return Elevator.getInstance().getElevatorHeightPercentage() > 0.10;
+			}
+		});
+
+		addSequential(new Command() {
+
+			@Override
+			protected boolean isFinished() {
+				return Elevator.getInstance().getLimitSwitch();
+			}
+
+			@Override
+			protected void initialize() {
+				Elevator.getInstance().setRaw(-0.2);
+			}
+
+			@Override
+			protected void end() {
+				Elevator.getInstance().setRaw(0);
+				Elevator.getInstance().zero();
+			}
+		});
+	}
+
+}

src/com/team3925/frc2018/commands/SetLiftTop.java

@@ -0,0 +1,20 @@
+package com.team3925.frc2018.commands;
+
+import com.team3925.frc2018.subsystems.Intake;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class SetLiftTop extends Command{
+
+	@Override
+	protected void initialize() {
+		Intake.getInstance().setAngle(85);
+	}
+
+	@Override
+	protected boolean isFinished() {
+		return true;
+	}
+
+
+}

src/com/team3925/frc2018/commands/ZeroIntake.java

@@ -13,17 +13,18 @@ protected boolean isFinished() {
 	}
 
 	@Override
-	protected void end() {
-		// stop motor
-		Intake.getInstance().zeroLift();
-		Intake.getInstance().setLiftMotorRaw(0);
+	protected void execute() {
+		Intake.getInstance().setLiftMotorRaw(-0.2);
 
 	}
 
 	@Override
-	protected void execute() {
-		Intake.getInstance().setLiftMotorRaw(-0.2);
+	protected void end() {
+		// stop motor
+		Intake.getInstance().zeroLift();
+		Intake.getInstance().setLiftMotorRaw(0);
+		Intake.getInstance().setAngle(85);
 
 	}
 
-}// end of class
+}

src/com/team3925/frc2018/commands/Center_Left_Switch_Auto.java → src/com/team3925/frc2018/commands/autos/CenterLeftSwitchAuto.java

@@ -1,14 +1,19 @@
-package com.team3925.frc2018.commands;
+package com.team3925.frc2018.commands.autos;
 
+import com.team3925.frc2018.commands.OpenGrabbers;
+import com.team3925.frc2018.commands.RunElevator;
+import com.team3925.frc2018.commands.RunIntakeWheels;
+import com.team3925.frc2018.commands.ZeroIntake;
 import com.team3925.frc2018.subsystems.Elevator.ElevatorState;
 import com.team3925.utils.MotionProfileCommand;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 import edu.wpi.first.wpilibj.command.WaitCommand;
 
-public class Center_Left_Switch_Auto extends CommandGroup{
+public class CenterLeftSwitchAuto extends CommandGroup{
 
-	public Center_Left_Switch_Auto() {
+	public CenterLeftSwitchAuto() {
+		addSequential(new ZeroIntake());
 		addParallel(new RunElevator(ElevatorState.BOTTOM));
 		addParallel(new MotionProfileCommand("CENTER_LEFTSWITCH"));
 		addSequential(new WaitCommand(2));

src/com/team3925/frc2018/commands/autos/CenterRightSwitchAuto.java

@@ -0,0 +1,26 @@
+package com.team3925.frc2018.commands.autos;
+
+import com.team3925.frc2018.commands.OpenGrabbers;
+import com.team3925.frc2018.commands.RunElevator;
+import com.team3925.frc2018.commands.RunIntakeWheels;
+import com.team3925.frc2018.commands.ZeroIntake;
+import com.team3925.frc2018.subsystems.Elevator.ElevatorState;
+import com.team3925.utils.MotionProfileCommand;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+public class CenterRightSwitchAuto extends CommandGroup{
+
+	public CenterRightSwitchAuto() {
+		addSequential(new ZeroIntake());
+		addParallel(new RunElevator(ElevatorState.BOTTOM));
+		addParallel(new MotionProfileCommand("CENTER_RIGHTSWITCH"));
+		addSequential(new WaitCommand(2));
+		addSequential(new RunElevator(ElevatorState.SWITCH));
+		addSequential(new OpenGrabbers());
+		addSequential(new RunIntakeWheels(-0.5));
+	}
+
+}
+

src/com/team3925/frc2018/subsystems/Elevator.java

@@ -26,10 +26,12 @@ public enum ElevatorState{
 	private static final double kP = 0.2; //0.015 | 0.2
 	private static final double kI = 0;
 	private static final double kD = 0;
-	private static final double kF = 0.35;
+	private static final double kF = 0.35; //0.35
 
 	private static final int MOTION_MAGIC_ACCELERATION = 9001;
-	private static final int MOTION_MAGIC_CRUISE_VELOCITY = 5467; //5467
+	private static final int MOTION_MAGIC_CRUISE_VELOCITY = 5467;
+//	private static final int MOTION_MAGIC_ACCELERATION = 9001;
+//	private static final int MOTION_MAGIC_CRUISE_VELOCITY = 5467;
 //	private static final int MOTION_MAGIC_ACCELERATION = 20594;
 //	private static final int MOTION_MAGIC_CRUISE_VELOCITY = 5467;
 
@@ -60,13 +62,13 @@ public Elevator() {
 		elevatorMaster.configMotionAcceleration(MOTION_MAGIC_ACCELERATION, Constants.TIMEOUT_MS);
 		elevatorMaster.configMotionCruiseVelocity(MOTION_MAGIC_CRUISE_VELOCITY, Constants.TIMEOUT_MS);
 
-		elevatorMaster.overrideLimitSwitchesEnable(true);
+		elevatorMaster.overrideLimitSwitchesEnable(false);
 		elevatorMaster.configContinuousCurrentLimit(200, Constants.TIMEOUT_MS);
 
-		elevatorMaster.configNominalOutputForward(1, Constants.TIMEOUT_MS);
-		elevatorMaster.configNominalOutputReverse(-1, Constants.TIMEOUT_MS);
-		RobotMap.ElevatorMap.SLAVE.configNominalOutputForward(1, Constants.TIMEOUT_MS);
-		RobotMap.ElevatorMap.SLAVE.configNominalOutputReverse(-1, Constants.TIMEOUT_MS);
+		elevatorMaster.configNominalOutputForward(0, Constants.TIMEOUT_MS);
+		elevatorMaster.configNominalOutputReverse(0, Constants.TIMEOUT_MS);
+		RobotMap.ElevatorMap.SLAVE.configNominalOutputForward(0, Constants.TIMEOUT_MS);
+		RobotMap.ElevatorMap.SLAVE.configNominalOutputReverse(0, Constants.TIMEOUT_MS);
 	}
 
 
@@ -79,6 +81,7 @@ public boolean isElevatorAtSetpoint() {
 	}
 
 	public void setPosition(double inches){
+//		elevatorMaster.set(ControlMode.Position, (inches * K_ELEVATOR));
 		elevatorMaster.set(ControlMode.MotionMagic, (Math.min(inches, MAX_SCALE_HEIGHT) * K_ELEVATOR));
 		elevatorHeight = inches;
 	}
@@ -89,19 +92,19 @@ public void setPosition(ElevatorState state){
 			setPosition((12 * 5.5) + 4.667);
 			break;
 		case SCALE_MAX:
-			setPosition(12 * 5.5);
+			setPosition(12 * 5);
 			break;
 		case SCALE_MED:
-			setPosition(12 * 5);
+			setPosition(12 * 4.5);
 			break;
 		case SCALE_LOW:
-			setPosition(12 * 4.5);
+			setPosition(12 * 4);
 			break;
 		case BOTTOM:
 			setPosition(3);
 			break;
 		case SWITCH:
-			setPosition(12 * 2.5);
+			setPosition(12 * 2);
 			break;
 		}
 	}