integration started with pnumatic intakes

drive dreain and ball and hatch intake

2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java

@@ -47,15 +47,31 @@ public class OI
 	        
 	        XBoxTriggerButton CarriageEject = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.RIGHT_TRIGGER);
 	        CarriageEject.whenPressed(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED));
-	        CarriageEject.whenReleased(new IntakeSetSpeed(0.0));
-	        */
-	        JoystickButton climbUp = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON);
-	        climbUp.whenPressed(new InitiateClimber(true));
-	        climbUp.whenReleased(new InitiateClimber(false));
+			CarriageEject.whenReleased(new IntakeSetSpeed(0.0));
+			*/
+			//JoystickButton endEfector = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON);
+			//endEfector.toggleWhenActive(new WristPlacement(true));
+
+
+
+		
+			JoystickButton liftHatchIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON);
+			liftHatchIntake.whenPressed(new LiftHatchDropBall());
+
+
+			JoystickButton liftBallIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON);
+			//liftBallIntake.whenPressed(new HatchFlip(false));
+			liftBallIntake.whenPressed(new LiftBallDropHatch());
+
+
+
+
+	       // climbUp.whenPressed(new InitiateClimber(true));
+	        //climbUp.whenReleased(new InitiateClimber(false));
 	        
 	        JoystickButton shiftUp = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON);
 	        shiftUp.whenPressed(new DriveSpeedShift(true));
-	       // shiftUp.whenPressed(new LEDIndicators(true));
+	        //shiftUp.whenPressed(new LEDIndicators(true));
 	        
 	        JoystickButton shiftDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON);
 	        shiftDown.whenPressed(new DriveSpeedShift(false));

2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java

@@ -32,7 +32,8 @@ public class Robot extends IterativeRobot
 
 
  
-    public static final Elevator elevator = new Elevator();
+	public static final Elevator elevator = new Elevator();
+	public static final BallIntake ballIntake = new BallIntake();
     
 
 
@@ -45,16 +46,6 @@ public class Robot extends IterativeRobot
 	public static OperationMode operationMode = OperationMode.TEST;
 
 	private SendableChooser<OperationMode> operationModeChooser;
-	private SendableChooser<Command> RRautonTaskChooser;
-	private SendableChooser<Command> RLautonTaskChooser;
-	private SendableChooser<Command> LRautonTaskChooser;
-	private SendableChooser<Command> LLautonTaskChooser;
-
-    private Command RRautonomousCommand;
-    private Command RLautonomousCommand;
-    private Command LRautonomousCommand;
-    private Command LLautonomousCommand;
-    
     public void robotInit() 
     {
     	//Printing game data to riolog
@@ -116,15 +107,17 @@ public class Robot extends IterativeRobot
     }
 
     public void teleopInit() {
-        if (RRautonomousCommand != null) RRautonomousCommand.cancel();
-        if (RLautonomousCommand != null) RLautonomousCommand.cancel();
-        if (LRautonomousCommand != null) LRautonomousCommand.cancel();
-        if (LLautonomousCommand != null) LLautonomousCommand.cancel();
+		System.err.println("Beginning of teleopInit.");
+
         drive.setToBrakeOnNeutral(false);	
-    	updateChoosers();
-        controlLoop.start();
-    	drive.resetEncoders();
-    	drive.endGyroCalibration();
+		updateChoosers();
+		System.err.println("TeleopInit after UpdateChoosers");
+		controlLoop.start();
+		System.err.println("TeleopInit after controlLoop");
+		//drive.resetEncoders();
+		//System.err.println("TeleopInit after resetEncoders");
+		drive.endGyroCalibration();
+		System.err.println("TeleopInit after endGyroCalibrations");
 
         updateStatus();
     }
@@ -143,10 +136,6 @@ public class Robot extends IterativeRobot
     
     private void updateChoosers() {
 		operationMode = (OperationMode)operationModeChooser.getSelected();
-		RRautonomousCommand = (Command)RRautonTaskChooser.getSelected();
-		RLautonomousCommand = (Command)RLautonTaskChooser.getSelected();
-		LRautonomousCommand = (Command)LRautonTaskChooser.getSelected();
-		LLautonomousCommand = (Command)LLautonTaskChooser.getSelected();
     }
     
     public Alliance getAlliance() {

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DeployBallIntake.java

@@ -0,0 +1,40 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class DeployBallIntake extends Command
+{
+	public boolean IsUp;
+	
+	public DeployBallIntake(boolean IsUp) {
+		this.IsUp=IsUp;
+		requires(Robot.pnumatics);
+	}
+
+	@Override
+	protected void initialize() {
+		Robot.pnumatics.setBallIntake(IsUp);
+	}
+
+	@Override
+	protected void execute() {
+		
+	}
+
+	@Override
+	protected boolean isFinished() {
+		return true;
+	}
+
+	@Override
+	protected void end() {
+		
+	}
+
+	@Override
+	protected void interrupted() {
+			
+	}
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/HatchFlip.java

@@ -0,0 +1,40 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class HatchFlip extends Command
+{
+	public boolean PickingUp;
+	
+	public HatchFlip(boolean PickingUP) {
+		this.PickingUp=PickingUP;
+		requires(Robot.pnumatics);
+	}
+
+	@Override
+	protected void initialize() {
+		Robot.pnumatics.setHatchIntakeState(PickingUp);
+	}
+
+	@Override
+	protected void execute() {
+		
+	}
+
+	@Override
+	protected boolean isFinished() {
+		return true;
+	}
+
+	@Override
+	protected void end() {
+		
+	}
+
+	@Override
+	protected void interrupted() {
+			
+	}
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/LiftBallDropHatch.java

@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package org.usfirst.frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+public class LiftBallDropHatch extends CommandGroup {
+  /**
+   * Add your docs here.
+   */
+  public LiftBallDropHatch() {
+    addSequential(new HatchFlip(false));
+    addParallel(new DeployBallIntake(false));
+    // Add Commands here:
+    // e.g. addSequential(new Command1());
+    // addSequential(new Command2());
+    // these will run in order.
+
+    // To run multiple commands at the same time,
+    // use addParallel()
+    // e.g. addParallel(new Command1());
+    // addSequential(new Command2());
+    // Command1 and Command2 will run in parallel.
+
+    // A command group will require all of the subsystems that each member
+    // would require.
+    // e.g. if Command1 requires chassis, and Command2 requires arm,
+    // a CommandGroup containing them would require both the chassis and the
+    // arm.
+  }
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/LiftHatchDropBall.java

@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package org.usfirst.frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+public class LiftHatchDropBall extends CommandGroup {
+  /**
+   * Add your docs here.
+   */
+  public LiftHatchDropBall() {
+    addSequential(new HatchFlip(true));
+    addParallel(new DeployBallIntake(true));
+    // Add Commands here:
+    // e.g. addSequential(new Command1());
+    // addSequential(new Command2());
+    // these will run in order.
+
+    // To run multiple commands at the same time,
+    // use addParallel()
+    // e.g. addParallel(new Command1());
+    // addSequential(new Command2());
+    // Command1 and Command2 will run in parallel.
+
+    // A command group will require all of the subsystems that each member
+    // would require.
+    // e.g. if Command1 requires chassis, and Command2 requires arm,
+    // a CommandGroup containing them would require both the chassis and the
+    // arm.
+  }
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetIntakeSpeed.java

@@ -0,0 +1,44 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+
+import org.usfirst.frc4388.robot.subsystems.BallIntake;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class SetIntakeSpeed extends Command {
+
+	private double WheelSpeed;
+	
+	// Constructor with speed
+    public SetIntakeSpeed(double WheelSpeed) {
+    	this.WheelSpeed = WheelSpeed;
+         requires(Robot.ballIntake);
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+    	Robot.ballIntake.setWheelSpeed(WheelSpeed);
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    protected void execute() {
+    }
+
+    // Make this return true when this Command no longer needs to run execute()
+    protected boolean isFinished() {
+        return true;
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristPlacement.java

@@ -0,0 +1,40 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class WristPlacement extends Command
+{
+	public boolean Forward;
+	
+	public WristPlacement(boolean Forward) {
+		this.Forward=Forward;
+		requires(Robot.pnumatics);
+	}
+
+	@Override
+	protected void initialize() {
+		Robot.pnumatics.setWrist(Forward);
+	}
+
+	@Override
+	protected void execute() {
+		
+	}
+
+	@Override
+	protected boolean isFinished() {
+		return true;
+	}
+
+	@Override
+	protected void end() {
+		
+	}
+
+	@Override
+	protected void interrupted() {
+			
+	}
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/BallIntake.java

@@ -0,0 +1,60 @@
+package org.usfirst.frc4388.robot.subsystems;
+
+import org.usfirst.frc4388.robot.RobotMap;
+import org.usfirst.frc4388.robot.commands.*;
+import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
+import org.usfirst.frc4388.utility.CANTalonEncoder;
+import org.usfirst.frc4388.utility.ControlLoopable;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+import org.usfirst.frc4388.robot.OI;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+
+
+
+/**
+ *
+ */
+public class BallIntake extends Subsystem {
+
+	private WPI_TalonSRX BallIntakeMain;
+	public static enum BallIntakeControlMode { JOYSTICK, MP_STRAIGHT, HOLD, MANUAL};
+	public static final double BALL_INTAKE_SPEED = 0.40;
+	public static final double BALL_EXTAKE_SPEED = -1.0;
+	public static final double CUBE_STOP_SPEED = 0;
+	/////^^^^^^^^^ replace this line with the modes we need
+	
+	
+	private boolean isFinished;
+	//private CarriageControlMode controlMode = CarriageControlMode.JOYSTICK;
+	
+	
+	
+	
+	public BallIntake() {
+		try {
+			BallIntakeMain = new WPI_TalonSRX(RobotMap.CLIMBER_CAN_ID);
+			//\][carriageLeft.set(CurrentLimit, value);
+			
+    }
+		catch (Exception e) {
+			System.err.println("An error occurred in the Ball Intake constructor");
+			
+			
+		}
+    }
+	
+		public void setWheelSpeed(double speed) {
+			BallIntakeMain.set(-speed);
+    }
+
+
+    @Override
+    public void periodic() {
+    	
+    }
+    	public void initDefaultCommand() {
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java

@@ -40,6 +40,7 @@ import com.kauailabs.navx.frc.AHRS;
 import com.revrobotics.CANEncoder;
 import com.revrobotics.CANPIDController;
 import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMax.IdleMode;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj.DoubleSolenoid;
@@ -192,13 +193,16 @@ public class Drive extends Subsystem implements ControlLoopable
     private AHRS gyroNavX;
 	private boolean useGyroLock;
 	private double gyroLockAngleDeg;
-	//private double kPGyro = 0.04;
-	private double kPGyro = 0.0625;
+	private double kPGyro = 0.07;
+	//private double kPGyro = 0.0625;
+	
 	private boolean isCalibrating = false;
 	private double gyroOffsetDeg = 0;
 
 	public Drive() {
 		try {
+			//System.err.println("Beginning of Drive.");
+
 			leftDrive1 = new CANSparkMax(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, MotorType.kBrushless);
 			//leftDrive1 = new CANSparkMax(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, false, FeedbackDevice.QuadEncoder);
 			leftDrive2 = new CANSparkMax(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID, MotorType.kBrushless);
@@ -209,6 +213,9 @@ public class Drive extends Subsystem implements ControlLoopable
 			rightDrive2 = new CANSparkMax(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID, MotorType.kBrushless);
 			encoderRight = new CANEncoder(rightDrive1);
 			rightDrive2.follow(rightDrive1);
+			
+			//System.err.println("After Constructors.");
+
 			//gyroPigeon = new PigeonImu(leftDrive2);
 			gyroNavX = new AHRS(SPI.Port.kMXP);
 			
@@ -218,17 +225,17 @@ public class Drive extends Subsystem implements ControlLoopable
 			//leftDrive1.clearStickyFaults();
 			//leftDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE);
 			//leftDrive1.setNominalClosedLoopVoltage(12.0);
-			leftDrive1.clearStickyFaults(0);
+			leftDrive1.clearFaults();
 			leftDrive1.setInverted(false);//false on comp 2108, false on microbot
-			leftDrive1.setSensorPhase(true);//true on comp 2108, false on microbot
-			leftDrive1.setSafetyEnabled(false);
+			//leftDrive1.setSensorPhase(true);//true on comp 2108, false on microbot //not needed for spark
+			//leftDrive1.setSafetyEnabled(false); //not needed for spark
 			//leftDrive1.setCurrentLimit(15);
 			//leftDrive1.enableCurrentLimit(true);
-			leftDrive1.setNeutralMode(NeutralMode.Brake);
-			leftDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0);
-			leftDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0);
-			leftDrive1.configNominalOutputForward(+1.0f, 0);
-			leftDrive1.configNominalOutputReverse(-1.0f, 0);
+			leftDrive1.setIdleMode(IdleMode.kBrake);
+			//leftDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0); // not needed for spark?
+			//leftDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0); // not needed for spark?
+			//leftDrive1.configNominalOutputForward(+1.0f, 0); // not needed for spark?
+			//leftDrive1.configNominalOutputReverse(-1.0f, 0); // not needed for spark?
 
 			
 //	        if (leftDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) {
@@ -237,40 +244,43 @@ public class Drive extends Subsystem implements ControlLoopable
 			
 
 			leftDrive2.setInverted(false);//false on comp 2108, false on microbot
-			leftDrive2.setSafetyEnabled(false);
-			leftDrive2.setNeutralMode(NeutralMode.Brake);
-			leftDrive2.set(ControlMode.Follower, leftDrive1.getDeviceID());
+			//leftDrive2.setSafetyEnabled(false);
+			leftDrive2.setIdleMode(IdleMode.kBrake);
+			//leftDrive2.set(ControlMode.Follower, leftDrive1.getDeviceID()); // set above
 	
 
 			
 			//rightDrive1.clearStickyFaults();
 			//rightDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE);
 			//rightDrive1.setNominalClosedLoopVoltage(12.0);
-			rightDrive1.clearStickyFaults(0);
-			rightDrive1.setInverted(true);//true on comp 2108, false on microbot
-			rightDrive1.setSensorPhase(true);//true on comp 2108, true on microbot
-			rightDrive1.setSafetyEnabled(false);
-			rightDrive1.setNeutralMode(NeutralMode.Brake);
-			rightDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0);
-			rightDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0);
-			rightDrive1.configNominalOutputForward(+1.0f, 0);
-			rightDrive1.configNominalOutputReverse(-1.0f, 0);
+			rightDrive1.clearFaults();
+			rightDrive1.setInverted(false);//true on comp 2108, false on microbot
+			//rightDrive1.setSensorPhase(true);//true on comp 2108, true on microbot
+			//rightDrive1.setSafetyEnabled(false);
+			rightDrive1.setIdleMode(IdleMode.kBrake);
+			//rightDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0);
+			//rightDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0);
+			//rightDrive1.configNominalOutputForward(+1.0f, 0);
+			//rightDrive1.configNominalOutputReverse(-1.0f, 0);
 //	        if (rightDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) {
 //	            DriverStation.reportError("Could not detect right drive encoder encoder!", false);
 //	        }
 			
 
-			rightDrive2.setInverted(true);//True on comp 2108, false on microbot
-			rightDrive2.setSafetyEnabled(false);
-			rightDrive2.setNeutralMode(NeutralMode.Brake);
-			rightDrive2.set(ControlMode.Follower, rightDrive1.getDeviceID());
+			rightDrive2.setInverted(false);//True on comp 2108, false on microbot
+			//rightDrive2.setSafetyEnabled(false);
+			rightDrive2.setIdleMode(IdleMode.kBrake);
+			//rightDrive2.set(ControlMode.Follower, rightDrive1.getDeviceID());
 
-
-			leftPIDController = new CANPIDController(leftDrive1);
-			rightPIDController = new CANPIDController(rightDrive1);
+			//System.err.println("After motor settings.");
+			
+			CANPIDController leftDrive1_Controller = new CANPIDController(leftDrive1);
+			CANPIDController rightDrive1_Controller = new CANPIDController(rightDrive1);
 							
-			motorControllers.add(leftPIDController);
-			motorControllers.add(rightPIDController);
+			motorControllers.add(leftDrive1_Controller);
+			motorControllers.add(rightDrive1_Controller);
+
+			//System.err.println("After motorControllers.");
 
 			//m_drive = new RobotDrive(leftDrive1, rightDrive1);
 			//m_drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
@@ -281,8 +291,11 @@ public class Drive extends Subsystem implements ControlLoopable
 			//m_drive.setInvertedMotor(DifferentialDrive.MotorType.kRearRight, true);	//TODO URGENT: verify
 			m_drive.setSafetyEnabled(false);
 			
+		
 			//speedShift = new Solenoid(RobotMap.DRIVETRAIN_SPEEDSHIFT_PCM_ID);
+			//System.err.println("End of Drive.");
 		}
+
 		catch (Exception e) {
 			System.err.println("An error occurred in the DriveTrain constructor");
 		}
@@ -290,15 +303,15 @@ public class Drive extends Subsystem implements ControlLoopable
 
 	public void setToBrakeOnNeutral(boolean brakeVsCoast) {
 		if (brakeVsCoast) {
-			leftDrive1.setNeutralMode(NeutralMode.Brake);
-			leftDrive2.setNeutralMode(NeutralMode.Brake);
-			rightDrive1.setNeutralMode(NeutralMode.Brake);
-			rightDrive2.setNeutralMode(NeutralMode.Brake);
+			leftDrive1.setIdleMode(IdleMode.kBrake);
+			leftDrive2.setIdleMode(IdleMode.kBrake);
+			rightDrive1.setIdleMode(IdleMode.kBrake);
+			rightDrive2.setIdleMode(IdleMode.kBrake);
 		} else {
-			leftDrive1.setNeutralMode(NeutralMode.Coast);
-			leftDrive2.setNeutralMode(NeutralMode.Coast);
-			rightDrive1.setNeutralMode(NeutralMode.Coast);
-			rightDrive2.setNeutralMode(NeutralMode.Coast);
+			leftDrive1.setIdleMode(IdleMode.kCoast);
+			leftDrive2.setIdleMode(IdleMode.kCoast);
+			rightDrive1.setIdleMode(IdleMode.kCoast);
+			rightDrive2.setIdleMode(IdleMode.kCoast);
 		}
 	}
 	
@@ -366,8 +379,7 @@ public class Drive extends Subsystem implements ControlLoopable
 		mpStraightController.setMPStraightTarget(0, distanceInches, maxVelocity, MP_STRAIGHT_T1, MP_STRAIGHT_T2, useGyroLock, yawAngle, true); 
 		setControlMode(DriveControlMode.MP_STRAIGHT);
 	}
-	
-	//public void setStraightMPCached(String key, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) {
+		//public void setStraightMPCached(String key, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) {
 	//	double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg();
 	//	mpStraightController.setPID(mpStraightPIDParams);
 	//	mpStraightController.setMPStraightTarget(key, useGyroLock, yawAngle, true); 
@@ -437,8 +449,8 @@ public class Drive extends Subsystem implements ControlLoopable
 	}
     
     public void updatePose() {
-        double left_distance = leftDrive1.getPositionWorld();
-        double right_distance = rightDrive1.getPositionWorld();
+        double left_distance = encoderLeft.getPosition();
+        double right_distance = encoderRight.getPosition();
         Rotation2d gyro_angle = Rotation2d.fromDegrees(-getGyroAngleDeg());
         lastPose = currentPose;
         currentPose = generateOdometryFromSensors(left_distance - left_encoder_prev_distance_, right_distance - right_encoder_prev_distance_, gyro_angle);
@@ -470,20 +482,20 @@ public class Drive extends Subsystem implements ControlLoopable
 		if (controlMode == DriveControlMode.JOYSTICK) {
 			//leftDrive1.changeControlMode(TalonControlMode.PercentVbus);
 			//rightDrive1.changeControlMode(TalonControlMode.PercentVbus);
-			leftDrive1.set(ControlMode.PercentOutput, 0);	//TODO URGENT: make sure not called when robot moving
-			rightDrive1.set(ControlMode.PercentOutput, 0);	//TODO URGENT: make sure not called when robot moving
+			leftDrive1.set(0);	//TODO URGENT: make sure not called when robot moving
+			rightDrive1.set(0);	//TODO URGENT: make sure not called when robot moving
 		}
 		else if (controlMode == DriveControlMode.MANUAL) {
 			//leftDrive1.changeControlMode(TalonControlMode.PercentVbus);
 			//rightDrive1.changeControlMode(TalonControlMode.PercentVbus);
-			leftDrive1.set(ControlMode.PercentOutput, 0);	//TODO URGENT: make sure not called when robot moving
-			rightDrive1.set(ControlMode.PercentOutput, 0);	//TODO URGENT: make sure not called when robot moving
+			leftDrive1.set(0);	//TODO URGENT: make sure not called when robot moving
+			rightDrive1.set(0);	//TODO URGENT: make sure not called when robot moving
 		}
 		else if (controlMode == DriveControlMode.CLIMB) {
 			//leftDrive1.changeControlMode(TalonControlMode.PercentVbus);
 			//rightDrive1.changeControlMode(TalonControlMode.PercentVbus);
-			leftDrive1.set(ControlMode.PercentOutput, 0);	//TODO URGENT: make sure not called when robot moving
-			rightDrive1.set(ControlMode.PercentOutput, 0);	//TODO URGENT: make sure not called when robot moving
+			leftDrive1.set(0);	//TODO URGENT: make sure not called when robot moving
+			rightDrive1.set(0);	//TODO URGENT: make sure not called when robot moving
 		}
 		else if (controlMode == DriveControlMode.HOLD) {
 			mpStraightController.setPID(mpHoldPIDParams);
@@ -493,10 +505,10 @@ public class Drive extends Subsystem implements ControlLoopable
 			//rightDrive1.changeControlMode(TalonControlMode.Position);
 			//rightDrive1.setPosition(0);
 			//rightDrive1.set(0);
-			leftDrive1.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
-			leftDrive1.set(ControlMode.Position, 0);
-			rightDrive1.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
-			rightDrive1.set(ControlMode.Position, 0);
+			//leftDrive1.setSelectedSensorPosition(0, 0, 0);	//not needed for spark?  TODO: verify want 0="Primary closed-loop", with no timeout
+			leftDrive1.set(0);
+			//rightDrive1.setSelectedSensorPosition(0, 0, 0);	//not needed for spark?  TODO: verify want 0="Primary closed-loop", with no timeout
+			rightDrive1.set(0);
 		}
 		isFinished = false;
 	}
@@ -646,7 +658,7 @@ public class Drive extends Subsystem implements ControlLoopable
 			m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim,
 					m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY);
 		}
-		
+
 		m_drive.arcadeDrive(m_moveOutput, m_steerOutput*.75);
 		// break;
 		// case CONTROLLER_XBOX_ARCADE_RIGHT:
@@ -680,14 +692,10 @@ public class Drive extends Subsystem implements ControlLoopable
 
 	public void rawDriveLeftRight(double leftPercentOutput, double rightPercentOutput) {
 		
-			if (elevatorRight.getSelectedSensorPosition(0) >= 3550) {
-				leftDrive1.set(ControlMode.PercentOutput, leftPercentOutput*.5);
-				rightDrive1.set(ControlMode.PercentOutput, rightPercentOutput*.5);
-			}
-			else /*(elevatorRight.getSelectedSensorPosition(0) < 3550)*/ {
-				leftDrive1.set(ControlMode.PercentOutput, leftPercentOutput);
-				rightDrive1.set(ControlMode.PercentOutput, rightPercentOutput);
-			}
+			
+				leftDrive1.set(leftPercentOutput);
+				rightDrive1.set(rightPercentOutput);
+			
 	}
 
 	private boolean inDeadZone(double input) {
@@ -759,9 +767,8 @@ public class Drive extends Subsystem implements ControlLoopable
 	}
 	
 	@Override
-	
 	public void setPeriodMs(long periodMs) {
-		/*
+		/*                                       FIX TODAY
 		mpStraightController = new MPTalonPIDController(periodMs, mpStraightPIDParams, motorControllers);
 		mpTurnController = new MPSoftwarePIDController(periodMs, mpTurnPIDParams, motorControllers);
 		pidTurnController = new SoftwarePIDController(pidTurnPIDParams, motorControllers);
@@ -801,23 +808,24 @@ public class Drive extends Subsystem implements ControlLoopable
     }
 
     public double getLeftPositionWorld() {
-    	return leftDrive1.getPositionWorld();
+    	return 0;//leftDrive1.getPositionWorld(); FIX TODAY
     }
 
     public double getRightPositionWorld() {
-    	return rightDrive1.getPositionWorld();
+    	return 0;//rightDrive1.getPositionWorld(); FIX TODAY
     }
 
 	public void updateStatus(Robot.OperationMode operationMode) {
 		if (operationMode == Robot.OperationMode.TEST) {
 			try {
+				SmartDashboard.putNumber("Gyro Value", getGyroAngleDeg());
 				SmartDashboard.putNumber("Update Period (ms)", lastControlLoopUpdatePeriod * 1000.0);
-				SmartDashboard.putNumber("Right Pos Ticks", rightDrive1.getSelectedSensorPosition(0));
-				SmartDashboard.putNumber("Left Pos Ticks", leftDrive1.getSelectedSensorPosition(0));
-				SmartDashboard.putNumber("Right Pos Inches", rightDrive1.getPositionWorld());
-				SmartDashboard.putNumber("Left Pos Inches", leftDrive1.getPositionWorld());
-				SmartDashboard.putNumber("Right Vel Ft-Sec", rightDrive1.getVelocityWorld() / 12);
-				SmartDashboard.putNumber("Left Vel Ft-Sec", leftDrive1.getVelocityWorld() / 12);
+				SmartDashboard.putNumber("Right Pos Ticks", 0);//rightDrive1.getSelectedSensorPosition(0));   FIX TODAY
+				SmartDashboard.putNumber("Left Pos Ticks", 0);//leftDrive1.getSelectedSensorPosition(0));
+				SmartDashboard.putNumber("Right Pos Inches", -encoderRight.getPosition());//rightDrive1.getPositionWorld());
+				SmartDashboard.putNumber("Left Pos Inches", encoderLeft.getPosition());//leftDrive1.getPositionWorld());
+				SmartDashboard.putNumber("Right Vel Ft-Sec", 0);//rightDrive1.getVelocityWorld() / 12);
+				SmartDashboard.putNumber("Left Vel Ft-Sec", 0);//leftDrive1.getVelocityWorld() / 12);
 				//SmartDashboard.putNumber("Left 1 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID));
 				//SmartDashboard.putNumber("Left 2 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID));
 //				SmartDashboard.putNumber("Left 3 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR3_CAN_ID));

2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Elevator.java

@@ -217,7 +217,7 @@ public class Elevator extends Subsystem implements ControlLoopable
     	
     	double elevatorPos = getEncoderElevatorPosition();
     	
-    	moveElevatorInput = Robot.oi.getOperatorController().getLeftYAxis();
+    	//moveElevatorInput = Robot.oi.getOperatorController().getLeftYAxis();
     	
     	//double moveElevatorSensitivity = adjustJoystickSensitivity(moveScale, moveTrim, moveElevatorInput, moveNonLinear, MOVE_NON_LINEARITY);
     	
@@ -226,11 +226,11 @@ public class Elevator extends Subsystem implements ControlLoopable
     	
     	if(elevatorTuningPressed == true)
       	{     		
-     		elevatorRight.set(moveElevatorInput * 0.5);
+     		//elevatorRight.set(moveElevatorInput * 0.5);
       	}
      	else if(elevatorTuningPressed == false)
      	{
-     		elevatorRight.set(moveElevatorInput);
+     		//elevatorRight.set(moveElevatorInput);
      	}
      		
      		/*

2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Pnumatics.java

@@ -10,13 +10,17 @@ public class Pnumatics extends Subsystem {
     
 		
 	private DoubleSolenoid speedShift;
-	private DoubleSolenoid Intake;
+	private DoubleSolenoid hatchIntake;
+	private DoubleSolenoid ballIntake;
+	private DoubleSolenoid wrist;
 
 	
 	public Pnumatics() {
 		try {
 			speedShift = new DoubleSolenoid(1,0,1);					
-			Intake = new DoubleSolenoid(1,4,5 ); 
+			hatchIntake = new DoubleSolenoid(1,2,3); 
+			ballIntake = new DoubleSolenoid(1,4,5);
+			wrist = new DoubleSolenoid(1,6,7);
 		} 
 		catch (Exception e) {
 			System.err.println("An error occurred in the Pnumatics constructor");
@@ -31,12 +35,28 @@ public class Pnumatics extends Subsystem {
 			speedShift.set(DoubleSolenoid.Value.kReverse);
 		}
 	}
-	public void setIntake(boolean state) {
+	public void setHatchIntakeState(boolean state) {
 		if (state==true) {
-			Intake.set(DoubleSolenoid.Value.kForward);
+			hatchIntake.set(DoubleSolenoid.Value.kForward);
 		}
 		if (state==false) {
-			Intake.set(DoubleSolenoid.Value.kReverse);
+			hatchIntake.set(DoubleSolenoid.Value.kReverse);
+		}
+	}
+	public void setBallIntake(boolean state) {
+		if (state==true) {
+			ballIntake.set(DoubleSolenoid.Value.kForward);
+		}
+		if (state==false) {
+			ballIntake.set(DoubleSolenoid.Value.kReverse);
+		}
+	}
+	public void setWrist(boolean state) {
+		if (state==true) {
+			wrist.set(DoubleSolenoid.Value.kForward);
+		}
+		if (state==false) {
+			wrist.set(DoubleSolenoid.Value.kReverse);
 		}
 	}