Fix the lift system

src/org/usfirst/frc/team1002/robot/Robot.java

@@ -6,8 +6,10 @@
 import org.usfirst.frc.team1002.robot.subsystems.Drive;
 import org.usfirst.frc.team1002.robot.subsystems.ExtArmSystem;
 import org.usfirst.frc.team1002.robot.subsystems.ForkSystem;
+import org.usfirst.frc.team1002.robot.subsystems.LiftSystem;
 
 import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Scheduler;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
@@ -16,18 +18,19 @@
 public class Robot extends IterativeRobot {
 
 	// Static instances of systems
+	public static final Joystick joystickMove = new Joystick(RobotMap.stick[0]);
 	public static final Drive drive = new Drive();
 	public static final ForkSystem forkSystem = new ForkSystem();
 	public static final LiftSystem liftSystem = new LiftSystem();
 	public static final ExtArmSystem extArmSystem = new ExtArmSystem();
 	public static final Dashboard dash = new Dashboard();
-	public static OI oi;
 
+	// Secondary handlers
+	public static OI oi;
 	// Camera
 	public static AxisCamera camera;
-
-	// Secondary handlers
 	private Command auto;
+
 	private Command publishDash;
 
 	@Override
@@ -60,6 +63,12 @@ public void teleopInit() {
 		Scheduler.getInstance().add(publishDash);
 	}
 
+	@Override
+	public void teleopPeriodic() {
+		Scheduler.getInstance().run();
+		Drive.move(joystickMove);
+	}
+
 	@Override
 	public void testPeriodic() {
 		LiveWindow.run();

src/org/usfirst/frc/team1002/robot/commands/Lift.java

@@ -1,6 +1,7 @@
 package org.usfirst.frc.team1002.robot.commands;
 
 import org.usfirst.frc.team1002.robot.Robot;
+import org.usfirst.frc.team1002.robot.subsystems.LiftSystem;
 
 public class Lift extends MotorCommand {
 

src/org/usfirst/frc/team1002/robot/commands/PublishValues.java

@@ -3,6 +3,7 @@
 import org.usfirst.frc.team1002.robot.Robot;
 import org.usfirst.frc.team1002.robot.subsystems.Dashboard;
 import org.usfirst.frc.team1002.robot.subsystems.ForkSystem;
+import org.usfirst.frc.team1002.robot.subsystems.LiftSystem;
 
 import edu.wpi.first.wpilibj.command.Command;
 

src/org/usfirst/frc/team1002/robot/subsystems/LiftSystem.java

@@ -0,0 +1,37 @@
+package org.usfirst.frc.team1002.robot.subsystems;
+
+import org.usfirst.frc.team1002.robot.RobotMap;
+import org.usfirst.frc.team1002.robot.commands.Lift;
+
+import edu.wpi.first.wpilibj.CANTalon;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+public class LiftSystem extends Subsystem {
+	public static CANTalon liftMotor;
+
+	public static DigitalInput limitSensorTop;
+	public static DigitalInput limitSensorBot;
+
+	/**
+	 * Internal lift speed setter. Use the Lift command instead.
+	 *
+	 * @param speed Internal speed value.
+	 */
+	public static void lift(double speed) {
+		if (limitSensorBot.get() && speed < 0 || limitSensorTop.get() && speed > 0 && !Dashboard.getButton(0)) speed = 0;
+		liftMotor.set(speed);
+	}
+
+	public LiftSystem() {
+		liftMotor = new CANTalon(RobotMap.liftMotor);
+		limitSensorTop = new DigitalInput(RobotMap.limitSwitches[0]);
+		limitSensorBot = new DigitalInput(RobotMap.limitSwitches[1]);
+		lift(0);
+	}
+
+	@Override
+	public void initDefaultCommand() {
+		setDefaultCommand(new Lift(0));
+	}
+}