From 6efa60ec6b7197f471df1915c5197d6866a8b432 Mon Sep 17 00:00:00 2001 From: Dave Staudacher Date: Mon, 5 Mar 2018 18:57:23 -0700 Subject: [PATCH] Updated snapshot as of 20180305-1715 --- src/org/usfirst/frc4388/robot/Constants.java | 12 +- src/org/usfirst/frc4388/robot/OI.java | 5 +- src/org/usfirst/frc4388/robot/Robot.java | 121 +++++++++++++++--- .../frc4388/robot/commands/ElevatorBasic.java | 68 +++++----- .../robot/commands/ElevatorSetDeploy.java | 8 +- .../robot/commands/ElevatorSetSpeed.java | 4 +- .../BlueGearLoadingSideForwardAuton.java | 49 ------- ...ightSwitchForward.java => CenterLeft.java} | 28 ++-- .../commands/auton/CrossTheBaseLine.java | 34 +++++ .../commands/auton/RightStartLeftScore.java | 14 +- .../commands/auton/RightSwitchAuton.java | 12 +- .../frc4388/robot/subsystems/Elevator.java | 78 +++++++---- .../robot/subsystems/ElevatorAuton.java | 120 ----------------- 13 files changed, 276 insertions(+), 277 deletions(-) delete mode 100644 src/org/usfirst/frc4388/robot/commands/auton/BlueGearLoadingSideForwardAuton.java rename src/org/usfirst/frc4388/robot/commands/auton/{RightSwitchForward.java => CenterLeft.java} (64%) create mode 100644 src/org/usfirst/frc4388/robot/commands/auton/CrossTheBaseLine.java delete mode 100644 src/org/usfirst/frc4388/robot/subsystems/ElevatorAuton.java diff --git a/src/org/usfirst/frc4388/robot/Constants.java b/src/org/usfirst/frc4388/robot/Constants.java index 9c23357..71e8706 100644 --- a/src/org/usfirst/frc4388/robot/Constants.java +++ b/src/org/usfirst/frc4388/robot/Constants.java @@ -21,13 +21,21 @@ public class Constants { public static double kDriveLowGearMaxSpeedInchesPerSec = 12.0 * 5.0; public static double kDriveStraightBasicMaxSpeedInchesPerSec = 72.0; public static double kDriveStraightBasicMinSpeedInchesPerSec = 5.0; - public static double kDriveTurnBasicTankMotorOutput = 0.2; + public static double kDriveTurnBasicTankMotorOutput = 0.5; public static double kDriveTurnBasicSingleMotorOutput = 0.15; public static double kElevatorWheelDiameterInches = 1; // Encoders public static int kDriveEncoderTicksPerRev = 4096; public static double kDriveEncoderTicksPerInch = (double)kDriveEncoderTicksPerRev / (kDriveWheelDiameterInches * Math.PI); - public static double kElevatorEncoderTicksPerInch = (double)kDriveEncoderTicksPerRev /(kElevatorWheelDiameterInches * Math.PI) ; + + // Elevator + public static int kElevatorEncoderTickesPerRev = 256; + public static double kElevatorInchesOfTravelPerRev = 3.75; +// public static double kElevatorEncoderTicksPerInch = (double)kElevatorEncoderTickesPerRev / kElevatorInchesOfTravelPerRev; + public static double kElevatorEncoderTicksPerInch = 126.36; + public static double kElevatorBasicPercentOutputUp = -.9; + public static double kElevatorBasicPercentOutputDown =.9; + // CONTROL LOOP GAINS // PID gains for drive velocity loop (LOW GEAR) diff --git a/src/org/usfirst/frc4388/robot/OI.java b/src/org/usfirst/frc4388/robot/OI.java index 3398359..7c8cb19 100644 --- a/src/org/usfirst/frc4388/robot/OI.java +++ b/src/org/usfirst/frc4388/robot/OI.java @@ -187,7 +187,10 @@ public class OI //SmartDashboard.putData("move intake", new IntakePosition(true)); //SmartDashboard.putData("move intake2", new IntakePosition(false)); //SmartDashboard.putData("Move Stop", new ElevatorSetSpeed(ElevatorAuton.RAISE_ELEVATOR_SPEED)); - SmartDashboard.putData("move elevator", new ElevatorBasic(20, 15)); + + + SmartDashboard.putData("move elevator", new ElevatorBasic(20)); + ///SmartDashboard.putData("Move Down", new ElevatorSetSpeed(ElevatorAuton.LOWER_ELEVATOR_SPEED)); ///SmartDashboard.putData("Move UP", new ElevatorSetSpeed(ElevatorAuton.STOP_ELEVATOR_SPEED)); diff --git a/src/org/usfirst/frc4388/robot/Robot.java b/src/org/usfirst/frc4388/robot/Robot.java index 2a76721..e5d4b98 100644 --- a/src/org/usfirst/frc4388/robot/Robot.java +++ b/src/org/usfirst/frc4388/robot/Robot.java @@ -33,7 +33,8 @@ import org.usfirst.frc4388.robot.subsystems.*; import org.usfirst.frc4388.utility.ControlLooper; import org.usfirst.frc4388.robot.subsystems.Drive; import org.usfirst.frc4388.robot.subsystems.Carriage; -import org.usfirst.frc4388.robot.subsystems.LED;; +import org.usfirst.frc4388.robot.subsystems.LED; +import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;; /** * The VM is configured to automatically run this class, and to call the * functions corresponding to each mode, as described in the TimedRobot @@ -50,7 +51,6 @@ public class Robot extends IterativeRobot //public static final Elevator elevator = new Elevator(); public static final Carriage carriage = new Carriage(); - public static final ElevatorAuton elevatorAuton = new ElevatorAuton(); public static final Elevator elevator = new Elevator(); @@ -72,9 +72,15 @@ public class Robot extends IterativeRobot public static OperationMode operationMode = OperationMode.TEST; private SendableChooser operationModeChooser; - private SendableChooser autonTaskChooser; + private SendableChooser RRautonTaskChooser; + private SendableChooser RLautonTaskChooser; + private SendableChooser LRautonTaskChooser; + private SendableChooser LLautonTaskChooser; - private Command autonomousCommand; + private Command RRautonomousCommand; + private Command RLautonomousCommand; + private Command LRautonomousCommand; + private Command LLautonomousCommand; /** * This function is run when the robot is first started up and should be @@ -109,14 +115,61 @@ public class Robot extends IterativeRobot operationModeChooser.addObject("Competition", OperationMode.COMPETITION); SmartDashboard.putData("Operation Mode", operationModeChooser); - autonTaskChooser = new SendableChooser(); - - //autonTaskChooser.addDefault("Blue Gear Loading Side Plus Travel Forward", new BlueGearLoadingSideForwardAuton()); - autonTaskChooser.addObject("Right Switch", new RightSwitchAuton()); - //autonTaskChooser.addObject("Right Switch Forward", new RightSwitchForward()); - autonTaskChooser.addDefault("RightStartLeftScore", new RightStartLeftScore()); - SmartDashboard.putData("Auton Task", autonTaskChooser); + + + + RRautonTaskChooser = new SendableChooser(); + + RRautonTaskChooser.addObject("Right Switch", new RightSwitchAuton()); + RRautonTaskChooser.addDefault("RR Cross The Base Line", new CrossTheBaseLine()); + RRautonTaskChooser.addObject("LeftSwitch Center", new CenterLeft()); + + + SmartDashboard.putData("RRAuton Task", RRautonTaskChooser); + + + + + + RLautonTaskChooser = new SendableChooser(); + + RLautonTaskChooser.addObject("Right Switch", new RightSwitchAuton()); + RLautonTaskChooser.addDefault("RL Cross The Base Line", new CrossTheBaseLine()); + RLautonTaskChooser.addObject("LeftSwitch Center", new CenterLeft()); + + + SmartDashboard.putData("RLAuton Task", RLautonTaskChooser); + + + + + + + LLautonTaskChooser = new SendableChooser(); + + LLautonTaskChooser.addObject("Right Switch", new RightSwitchAuton()); + LLautonTaskChooser.addDefault("LL Cross The Base Line", new CrossTheBaseLine()); + LLautonTaskChooser.addObject("LeftSwitch Center", new CenterLeft()); + + + SmartDashboard.putData("LLAuton Task", LLautonTaskChooser); + + + + + + LRautonTaskChooser = new SendableChooser(); + + LRautonTaskChooser.addObject("Right Switch", new RightSwitchAuton()); + LRautonTaskChooser.addDefault("LR Cross The Base Line", new CrossTheBaseLine()); + LRautonTaskChooser.addObject("LeftSwitch Center", new CenterLeft()); + + + SmartDashboard.putData("LRAuton Task", LRautonTaskChooser); + + + //ledLights.setAllLightsOn(false); @@ -158,14 +211,36 @@ public class Robot extends IterativeRobot //elevator.resetElevatorEncoder(); drive.resetGyro(); drive.setIsRed(getAlliance().equals(Alliance.Red)); - if (autonomousCommand != null) autonomousCommand.start(); - } - - /** - * This function is called periodically during autonomous - */ + + + String gameData; + gameData = DriverStation.getInstance().getGameSpecificMessage(); + if(gameData.length() > 0) + { + if(gameData.charAt(0) == 'L') + { + if(gameData.charAt(1) == 'L') + { + if (LLautonomousCommand != null) LLautonomousCommand.start(); + } else { + if (LRautonomousCommand != null) LRautonomousCommand.start(); + } + } + } else { + if(gameData.charAt(1) == 'L') + { + if (RLautonomousCommand != null) RLautonomousCommand.start(); + } else { + if (RRautonomousCommand != null) RRautonomousCommand.start(); + } + } + + } + + public void autonomousPeriodic() { Scheduler.getInstance().run(); + //Robot.elevator.setControlMode(DriveControlMode.RAW); updateStatus(); } @@ -174,7 +249,10 @@ public class Robot extends IterativeRobot // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove // this line or comment it out. - if (autonomousCommand != null) autonomousCommand.cancel(); + if (RRautonomousCommand != null) RRautonomousCommand.cancel(); + if (RLautonomousCommand != null) RLautonomousCommand.cancel(); + if (LRautonomousCommand != null) LRautonomousCommand.cancel(); + if (LLautonomousCommand != null) LLautonomousCommand.cancel(); //MotionProfileCache.getInstance().release(); updateChoosers(); controlLoop.start(); @@ -185,6 +263,7 @@ public class Robot extends IterativeRobot //shooterFeed.setSpeed(0); //zarkerFeed.setSpeed(0); //shooter.setHopperPosition(HopperState.CLOSE); + //Robot.elevator.setControlMode(DriveControlMode.JOYSTICK); updateStatus(); } @@ -207,7 +286,10 @@ public class Robot extends IterativeRobot private void updateChoosers() { operationMode = (OperationMode)operationModeChooser.getSelected(); - autonomousCommand = (Command)autonTaskChooser.getSelected(); + RRautonomousCommand = (Command)RRautonTaskChooser.getSelected(); + RLautonomousCommand = (Command)RLautonTaskChooser.getSelected(); + LRautonomousCommand = (Command)LRautonTaskChooser.getSelected(); + LLautonomousCommand = (Command)LLautonTaskChooser.getSelected(); } public Alliance getAlliance() { @@ -216,6 +298,7 @@ public class Robot extends IterativeRobot public void updateStatus() { drive.updateStatus(operationMode); + elevator.updateStatus(operationMode); //carriage.updateStatus(operationMode); //shooter.updateStatus(operationMode); //shooterFeed.updateStatus(operationMode); diff --git a/src/org/usfirst/frc4388/robot/commands/ElevatorBasic.java b/src/org/usfirst/frc4388/robot/commands/ElevatorBasic.java index 474065c..df2b718 100644 --- a/src/org/usfirst/frc4388/robot/commands/ElevatorBasic.java +++ b/src/org/usfirst/frc4388/robot/commands/ElevatorBasic.java @@ -10,36 +10,36 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class ElevatorBasic extends Command { - private double m_targetInches; - private double m_maxVelocityInchesPerSec; + private double m_targetHeightInchesAboveFloor; private double m_speed; - private boolean m_goingBackwards; + private boolean m_goingUp; private double m_commandInitTimestamp; private double m_lastCommandExecuteTimestamp = 0.0; private double m_lastCommandExecutePeriod = 0.0; + public static boolean isfinishedElevatorBasic; - public ElevatorBasic(double targetInches, double maxVelocityInchesPerSec) { + public ElevatorBasic(double targetHeightInchesAboveFloor) { requires(Robot.elevator); - m_targetInches = targetInches; - m_maxVelocityInchesPerSec = maxVelocityInchesPerSec; - m_goingBackwards = (m_targetInches < 0.0); + m_targetHeightInchesAboveFloor = targetHeightInchesAboveFloor; } - - protected double velocityToMoveSpeed(double velocityInchesPerSec, boolean backwards) { - double sign = (backwards ? -1.0 : 1.0); - // Keep velocity above stiction limit (else bot will freeze and command will not complete) - double velocity = Math.max(Constants.kDriveStraightBasicMinSpeedInchesPerSec, velocityInchesPerSec); - // Figure out move value based on percentage of measured max speed (i.e. that at full 1.0 joystick) - return sign * velocity / Constants.kDriveStraightBasicMaxSpeedInchesPerSec; - } // Called just before this Command runs the first time - protected void initialize() { - Robot.elevator.resetElevatorEncoder(); + protected void initialize() + { Robot.elevator.setControlMode(DriveControlMode.RAW); + + double currentHeight = Robot.elevator.getElevatorHeightInchesAboveFloor(); // start out at half speed, as crude way to reduce slippage - m_speed = velocityToMoveSpeed(m_maxVelocityInchesPerSec/2.0, m_goingBackwards); + m_goingUp = (m_targetHeightInchesAboveFloor > currentHeight); +System.out.println("initialize(): cur=" + currentHeight + " , target=" + m_targetHeightInchesAboveFloor + " , going " + (m_goingUp ? "UP" : "DOWN")); + if (m_goingUp) { + m_speed = Constants.kElevatorBasicPercentOutputUp; + } + else { + m_speed = Constants.kElevatorBasicPercentOutputDown; + } m_commandInitTimestamp = Timer.getFPGATimestamp(); + } // Called repeatedly when this Command is scheduled to run @@ -50,31 +50,31 @@ public class ElevatorBasic extends Command { m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp; } m_lastCommandExecuteTimestamp = currentTimestamp; - double steer = 0.0; - - - Robot.elevator.rawMoveSteer(m_speed, steer); + Robot.elevator.rawSetOutput(m_speed); //SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod); } // Make this return true when this Command no longer needs to run execute() protected boolean isFinished() { boolean finished=false; - double velocity = m_maxVelocityInchesPerSec; - double position = (Robot.elevator.getelevatorPositionWorld()); - double remaining = (m_targetInches - position) * (m_goingBackwards ? -1.0 : 1.0); + double currentHeight = Robot.elevator.getElevatorHeightInchesAboveFloor(); + double remaining = (m_targetHeightInchesAboveFloor - currentHeight) * (m_goingUp ? 1.0 : -1.0); +System.out.println("cur=" + currentHeight + " , remaining=" + remaining + " , target=" + m_targetHeightInchesAboveFloor); if (remaining < 0.0) { finished = true; - } else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) { // last 100 ms + + } + /*} else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) { // last 100 ms velocity = m_maxVelocityInchesPerSec / 4.0; // quarter speed } else if (remaining < 0.3 * m_maxVelocityInchesPerSec) { // last 300 ms velocity = m_maxVelocityInchesPerSec / 2.0; // half speed - } + }*/ + + if (!finished) { - m_speed = velocityToMoveSpeed(velocity, m_goingBackwards); - SmartDashboard.putNumber("DSB Dist", position); + SmartDashboard.putNumber("EB Dist", currentHeight); } else { - SmartDashboard.putNumber("DSB finDist", position); + SmartDashboard.putNumber("EB finDist", currentHeight); } return finished; } @@ -82,8 +82,12 @@ public class ElevatorBasic extends Command { // Called once after isFinished returns true protected void end() { double currentTimestamp = Timer.getFPGATimestamp(); - SmartDashboard.putNumber("DSB Runtime", currentTimestamp - m_commandInitTimestamp); - Robot.elevator.rawMoveSteer(0.0, 0.0); + SmartDashboard.putNumber("EB Runtime", currentTimestamp - m_commandInitTimestamp); + + isfinishedElevatorBasic = isFinished(); + + Robot.elevator.rawSetOutput(0.0); + Robot.elevator.setControlMode(DriveControlMode.JOYSTICK); } diff --git a/src/org/usfirst/frc4388/robot/commands/ElevatorSetDeploy.java b/src/org/usfirst/frc4388/robot/commands/ElevatorSetDeploy.java index 81104ac..2330795 100644 --- a/src/org/usfirst/frc4388/robot/commands/ElevatorSetDeploy.java +++ b/src/org/usfirst/frc4388/robot/commands/ElevatorSetDeploy.java @@ -2,7 +2,7 @@ package org.usfirst.frc4388.robot.commands; import org.usfirst.frc4388.robot.Robot; import org.usfirst.frc4388.robot.commands.IntakeSetPosition.IntakePosition; -import org.usfirst.frc4388.robot.subsystems.ElevatorAuton; +//import org.usfirst.frc4388.robot.subsystems.ElevatorAuton; import edu.wpi.first.wpilibj.command.Command; @@ -30,12 +30,12 @@ public class ElevatorSetDeploy extends Command { secondTimerSet = false; if (position == IntakePosition.CUBE_INTAKE) { this.setTimeout(timeout); - Robot.elevatorAuton.setRaiseSpeed(ElevatorAuton.LOWER_ELEVATOR_SPEED); + //Robot.elevatorAuton.setRaiseSpeed(ElevatorAuton.LOWER_ELEVATOR_SPEED); isDeploy = true; } else if (position == IntakePosition.CUBE_DEPLOY) { this.setTimeout(timeout); - Robot.elevatorAuton.setRaiseSpeed(ElevatorAuton.RAISE_ELEVATOR_SPEED); + //Robot.elevatorAuton.setRaiseSpeed(ElevatorAuton.RAISE_ELEVATOR_SPEED); isDeploy = true; } else { @@ -63,7 +63,7 @@ public class ElevatorSetDeploy extends Command { // Called once after isFinished returns true protected void end() { if (isDeploy == true) { - Robot.elevatorAuton.setRaiseSpeed(0); + //Robot.elevatorAuton.setRaiseSpeed(0); } } diff --git a/src/org/usfirst/frc4388/robot/commands/ElevatorSetSpeed.java b/src/org/usfirst/frc4388/robot/commands/ElevatorSetSpeed.java index 6e35db0..7553d48 100644 --- a/src/org/usfirst/frc4388/robot/commands/ElevatorSetSpeed.java +++ b/src/org/usfirst/frc4388/robot/commands/ElevatorSetSpeed.java @@ -16,12 +16,12 @@ public class ElevatorSetSpeed extends Command { // Constructor with speed public ElevatorSetSpeed(double RaiseSpeed) { this.RaiseSpeed = RaiseSpeed; - requires(Robot.elevatorAuton); + // requires(Robot.elevatorAuton); } // Called just before this Command runs the first time protected void initialize() { - Robot.elevatorAuton.setRaiseSpeed(RaiseSpeed); + //Robot.elevatorAuton.setRaiseSpeed(RaiseSpeed); } // Called repeatedly when this Command is scheduled to run diff --git a/src/org/usfirst/frc4388/robot/commands/auton/BlueGearLoadingSideForwardAuton.java b/src/org/usfirst/frc4388/robot/commands/auton/BlueGearLoadingSideForwardAuton.java deleted file mode 100644 index d6bf0e4..0000000 --- a/src/org/usfirst/frc4388/robot/commands/auton/BlueGearLoadingSideForwardAuton.java +++ /dev/null @@ -1,49 +0,0 @@ - -package org.usfirst.frc4388.robot.commands.auton; - -import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP; -import org.usfirst.frc4388.robot.commands.DriveGyroReset; -//import org.usfirst.frc4388.robot.commands.DriveSpeedShift; -import org.usfirst.frc4388.robot.commands.DriveStraightMP; -//import org.usfirst.frc4388.robot.commands.IntakeSetPosition; -//import org.usfirst.frc4388.robot.commands.IntakeSetPosition.IntakePosition; -//import org.usfirst.frc4388.robot.commands.ShooterSetRpm; -//import org.usfirst.frc4388.robot.commands.ShooterSetShotPosition; -//import org.usfirst.frc4388.robot.commands.ShooterSetVoltageRampRate; -import org.usfirst.frc4388.robot.subsystems.Drive; -//import org.usfirst.frc4388.robot.subsystems.Shooter; -//import org.usfirst.frc4388.robot.subsystems.Drive.SpeedShiftState; -//import org.usfirst.frc4388.robot.subsystems.Shooter.ShotState; -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; - -import edu.wpi.first.wpilibj.command.CommandGroup; -import edu.wpi.first.wpilibj.command.WaitCommand; - -/** - * - */ -public class BlueGearLoadingSideForwardAuton extends CommandGroup { - - public BlueGearLoadingSideForwardAuton() { - addSequential(new DriveGyroReset()); - //addSequential(new IntakeSetPosition(IntakePosition.GEAR_PRESENT)); - - //addSequential(new DriveStraightMP(60, 10, true, true, 0)); // 95 for 112" greenville - //addSequential(new DriveAbsoluteTurnMP(10, Drive.MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC, MPSoftwareTurnType.TANK)); - //addSequential(new DriveStraightMP(10, Drive.MP_GEAR_DEPLOY_FASTER_VELOCITY_INCHES_PER_SEC, true, true, -30)); - //addSequential(new IntakeSetPosition(IntakePosition.GEAR_DEPLOY)); - // addSequential(new WaitCommand(1.0)); - //addSequential(new DriveStraightMP(40, Drive.MP_AUTON_MAX_STRAIGHT_VELOCITY_INCHES_PER_SEC, true, true, -10)); // 95 for 112" greenville - //addSequential(new DriveAbsoluteTurnMP(10, Drive.MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC, MPSoftwareTurnType.TANK)); - /// addSequential(new DriveStraightMP(-10, Drive.MP_GEAR_DEPLOY_FASTER_VELOCITY_INCHES_PER_SEC, true, true, -30)); - - // addSequential(new DriveStraightMP(40, Drive.MP_AUTON_MAX_STRAIGHT_VELOCITY_INCHES_PER_SEC, true, true, 10)); - //addSequential(new IntakeSetPosition(IntakePosition.GEAR_DEPLOY)); - - // addSequential(new WaitCommand(0.3)); - // addSequential(new DriveStraightMP(-20, Drive.MP_AUTON_MAX_BOILER_STRAIGHT_VELOCITY_INCHES_PER_SEC, true, true, -60)); - // addSequential(new DriveAbsoluteTurnMP(-173, Drive.MP_AUTON_MAX_BOILER_TURN_RATE_DEG_PER_SEC, MPSoftwareTurnType.TANK)); -// addSequential(new DriveSpeedShift(SpeedShiftState.HI)); - // addSequential(new DriveStraightMP(-254, Drive.MP_AUTON_MAX_STRAIGHT_VELOCITY_INCHES_PER_SEC, true, true, -173)); - } -} diff --git a/src/org/usfirst/frc4388/robot/commands/auton/RightSwitchForward.java b/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java similarity index 64% rename from src/org/usfirst/frc4388/robot/commands/auton/RightSwitchForward.java rename to src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java index 595fb91..c6dc96a 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/RightSwitchForward.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java @@ -6,6 +6,8 @@ import org.usfirst.frc4388.robot.commands.DriveSpeedShift; import org.usfirst.frc4388.robot.commands.DriveStraightBasic; import org.usfirst.frc4388.robot.commands.DriveStraightMP; import org.usfirst.frc4388.robot.commands.DriveTurnBasic; +import org.usfirst.frc4388.robot.commands.ElevatorBasic; +import org.usfirst.frc4388.robot.commands.ElevatorSetSpeed; import org.usfirst.frc4388.robot.commands.IntakePosition; import org.usfirst.frc4388.robot.commands.IntakeSetSpeed; import org.usfirst.frc4388.robot.subsystems.Carriage; @@ -18,27 +20,25 @@ import edu.wpi.first.wpilibj.command.WaitCommand; /** * */ -public class RightSwitchForward extends CommandGroup { +public class CenterLeft extends CommandGroup { - public RightSwitchForward() { + public CenterLeft() { addSequential(new DriveGyroReset()); + addSequential(new IntakePosition(true)); - addSequential(new DriveStraightBasic(-25, 25, true, true, 0)); - addSequential(new DriveTurnBasic(true, 178, 90, MPSoftwareTurnType.TANK)); - addSequential(new DriveGyroReset()); - addSequential(new WaitCommand(.1)); - addSequential(new DriveStraightBasic(125, 30, true, true, 0)); - addSequential(new DriveGyroReset()); - addSequential(new WaitCommand(.1)); - addSequential(new DriveTurnBasic(true, -90, 90, MPSoftwareTurnType.TANK)); - //addSequential(new DriveStraightBasic(5, 20, true, true, 0)); + addSequential(new DriveStraightBasic(-10, 60, true, true, 0)); + addSequential(new DriveTurnBasic(true, 130, 300, MPSoftwareTurnType.TANK)); + addSequential(new DriveStraightBasic(50, 60, true, true, 0)); + addSequential(new DriveTurnBasic(true, 28, 300, MPSoftwareTurnType.TANK)); + addSequential(new ElevatorBasic(20)); + addSequential(new DriveStraightBasic(25, 60, true, true, 0)); addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); - addSequential(new WaitCommand(.6)); + //addSequential(new WaitCommand(.1)); addSequential(new IntakePosition(false)); - addSequential(new WaitCommand(.9)); + addSequential(new WaitCommand(.5)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); //addSequential(new DriveStraightMP(60, 10, true, true, 0)); // 95 for 112" greenville } -} \ No newline at end of file +} diff --git a/src/org/usfirst/frc4388/robot/commands/auton/CrossTheBaseLine.java b/src/org/usfirst/frc4388/robot/commands/auton/CrossTheBaseLine.java new file mode 100644 index 0000000..275f37a --- /dev/null +++ b/src/org/usfirst/frc4388/robot/commands/auton/CrossTheBaseLine.java @@ -0,0 +1,34 @@ +package org.usfirst.frc4388.robot.commands.auton; + +import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP; +import org.usfirst.frc4388.robot.commands.DriveGyroReset; +import org.usfirst.frc4388.robot.commands.DriveSpeedShift; +import org.usfirst.frc4388.robot.commands.DriveStraightBasic; +import org.usfirst.frc4388.robot.commands.DriveStraightMP; +import org.usfirst.frc4388.robot.commands.DriveTurnBasic; +import org.usfirst.frc4388.robot.commands.ElevatorBasic; +import org.usfirst.frc4388.robot.commands.IntakePosition; +import org.usfirst.frc4388.robot.commands.IntakeSetSpeed; +import org.usfirst.frc4388.robot.subsystems.Carriage; +import org.usfirst.frc4388.robot.subsystems.Drive; +import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.WaitCommand; + +/** + * + */ +public class CrossTheBaseLine extends CommandGroup { + + public CrossTheBaseLine() { + + addSequential(new DriveGyroReset()); + addSequential(new DriveSpeedShift(true)); + addSequential(new DriveSpeedShift(true)); + addSequential(new IntakePosition(true)); + addSequential(new DriveStraightBasic(-90, 60, true, true, 0)); + + + } +} diff --git a/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java b/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java index 8552272..ebc984e 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java @@ -6,6 +6,7 @@ import org.usfirst.frc4388.robot.commands.DriveSpeedShift; import org.usfirst.frc4388.robot.commands.DriveStraightBasic; import org.usfirst.frc4388.robot.commands.DriveStraightMP; import org.usfirst.frc4388.robot.commands.DriveTurnBasic; +import org.usfirst.frc4388.robot.commands.ElevatorBasic; import org.usfirst.frc4388.robot.commands.IntakePosition; import org.usfirst.frc4388.robot.commands.IntakeSetSpeed; import org.usfirst.frc4388.robot.subsystems.Carriage; @@ -21,19 +22,24 @@ import edu.wpi.first.wpilibj.command.WaitCommand; public class RightStartLeftScore extends CommandGroup { public RightStartLeftScore() { + addSequential(new DriveGyroReset()); + addSequential(new DriveSpeedShift(true)); + addSequential(new DriveSpeedShift(true)); addSequential(new IntakePosition(true)); - addSequential(new DriveStraightBasic(-210, 60, true, true, 0)); + addSequential(new DriveStraightBasic(-215, 60, true, true, 0)); + addSequential(new ElevatorBasic(5)); addSequential(new DriveTurnBasic(true, 87, 300, MPSoftwareTurnType.TANK)); addSequential(new DriveStraightBasic(150, 60, true, true, 0)); + addSequential(new ElevatorBasic(30)); addSequential(new DriveTurnBasic(true, -87, 300, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasic(5, 60, true, true, 0)); + addSequential(new DriveStraightBasic(3, 60, true, true, 0)); addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); //addSequential(new WaitCommand(.1)); addSequential(new IntakePosition(false)); - addSequential(new WaitCommand(.5)); + addSequential(new WaitCommand(.2)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); - + addSequential(new DriveStraightBasic(-10, 60, true, true, 0)); //addSequential(new DriveStraightMP(60, 10, true, true, 0)); // 95 for 112" greenville } diff --git a/src/org/usfirst/frc4388/robot/commands/auton/RightSwitchAuton.java b/src/org/usfirst/frc4388/robot/commands/auton/RightSwitchAuton.java index 8954014..928d60d 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/RightSwitchAuton.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/RightSwitchAuton.java @@ -6,12 +6,12 @@ import org.usfirst.frc4388.robot.commands.DriveSpeedShift; import org.usfirst.frc4388.robot.commands.DriveStraightBasic; import org.usfirst.frc4388.robot.commands.DriveStraightMP; import org.usfirst.frc4388.robot.commands.DriveTurnBasic; +import org.usfirst.frc4388.robot.commands.ElevatorBasic; import org.usfirst.frc4388.robot.commands.ElevatorSetSpeed; import org.usfirst.frc4388.robot.commands.IntakePosition; import org.usfirst.frc4388.robot.commands.IntakeSetSpeed; import org.usfirst.frc4388.robot.subsystems.Carriage; import org.usfirst.frc4388.robot.subsystems.Drive; -import org.usfirst.frc4388.robot.subsystems.ElevatorAuton; import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -24,13 +24,13 @@ public class RightSwitchAuton extends CommandGroup { public RightSwitchAuton() { addSequential(new DriveGyroReset()); + addSequential(new DriveSpeedShift(true)); addSequential(new IntakePosition(true)); - addSequential(new DriveStraightBasic(-150, 60, true, true, 0)); - addSequential(new ElevatorSetSpeed(1.00)); - addSequential(new WaitCommand(.5)); - addSequential(new ElevatorSetSpeed(0.00)); + addSequential(new DriveStraightBasic(-130, 60, true, true, 0)); + addSequential(new ElevatorBasic(30)); addSequential(new DriveTurnBasic(true, 87, 300, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasic(9, 60, true, true, 0)); + addSequential(new ElevatorBasic(30)); + addSequential(new DriveStraightBasic(20, 60, true, true, 0)); addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); //addSequential(new WaitCommand(.1)); addSequential(new IntakePosition(false)); diff --git a/src/org/usfirst/frc4388/robot/subsystems/Elevator.java b/src/org/usfirst/frc4388/robot/subsystems/Elevator.java index 1ef067d..a91b5bb 100644 --- a/src/org/usfirst/frc4388/robot/subsystems/Elevator.java +++ b/src/org/usfirst/frc4388/robot/subsystems/Elevator.java @@ -59,7 +59,7 @@ public class Elevator extends Subsystem implements ControlLoopable private double targetPPosition; //Encoder ticks to inches for encoders - public static final double ENCODER_TICKS_TO_INCHES = Constants.kDriveEncoderTicksPerInch; + public static final double ENCODER_TICKS_TO_INCHES = Constants.kElevatorEncoderTicksPerInch; //control mode for joystick control private DriveControlMode controlMode = DriveControlMode.JOYSTICK; @@ -82,6 +82,7 @@ public class Elevator extends Subsystem implements ControlLoopable public boolean pressed; SensorCollection isPressed; + public boolean elevatorControlMode = false; // Motor controllers //private ArrayList motorControllers = new ArrayList(); @@ -93,10 +94,11 @@ public class Elevator extends Subsystem implements ControlLoopable elevatorRight = new CANTalonEncoder(RobotMap.ELEVATOR_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); elevatorLeft = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR2_ID); - //Setting right elevator motor as follower + elevatorRight.setInverted(false); + + //Setting left elevator motor as follower elevatorLeft.set(ControlMode.Follower, elevatorRight.getDeviceID()); elevatorLeft.setInverted(false); - elevatorRight.setInverted(false); //Limit Switch Left elevatorLeft.overrideLimitSwitchesEnable(true); @@ -192,7 +194,7 @@ public class Elevator extends Subsystem implements ControlLoopable public void moveElevatorXbox() { double moveElevatorInput; - double elevatorSafeZone = 1.2; + double elevatorSafeZone = -30; double elevatorPos = getEncoderElevatorPosition(); @@ -205,19 +207,23 @@ public class Elevator extends Subsystem implements ControlLoopable if(elevatorTuningPressed == true) { - elevatorRight.set(moveElevatorInput * (0.5)); + elevatorRight.set(moveElevatorInput * 0.5); } else if(elevatorTuningPressed == false) { - if(elevatorPos <= elevatorSafeZone /*&& elevatorPos >= 0*/) + elevatorRight.set(moveElevatorInput); + } + + /* + if(elevatorPos <= elevatorSafeZone && elevatorPos >= 0) { - elevatorRight.set(moveElevatorInput * 0.65); + elevatorRight.set(moveElevatorInput); } else if(elevatorPos > elevatorSafeZone) { - elevatorRight.set(moveElevatorInput); + elevatorRight.set(moveElevatorInput * 0.65); + - /* if(holdButtonPressed == true) { elevatorRight.set(-0.43 * (0.2)); @@ -226,18 +232,20 @@ public class Elevator extends Subsystem implements ControlLoopable { elevatorRight.set(moveElevatorInput * 0.75); } - */ + } - /* + else if(elevatorPos < 0) { elevatorRight.set(moveElevatorInput * 0.75); } + */ - } - - System.out.println(elevatorPos); //-6.9 to 1.9 total: 8.8 range + + System.out.println(moveElevatorInput); } + +// System.out.println(elevatorPos); //-6.9 to 1.9 total: 8.8 range //PID encoder position public double getEncoderElevatorPosition() @@ -245,6 +253,11 @@ public class Elevator extends Subsystem implements ControlLoopable return elevatorRight.getPositionWorld(); } + public double getElevatorHeightInchesAboveFloor() + { + return elevatorRight.getPositionWorld(); + } + public synchronized void setControlMode(DriveControlMode controlMode) { this.controlMode = controlMode; @@ -284,6 +297,10 @@ public class Elevator extends Subsystem implements ControlLoopable Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_LOWEST); } */ + public void rawSetOutput(double output){ + elevatorRight.set(/*ControlMode.PercentOutput,*/ output); + } + public void holdInPos() { elevatorRight.set(-0.43 * 0.2); @@ -301,23 +318,30 @@ public class Elevator extends Subsystem implements ControlLoopable if(isPressed.isFwdLimitSwitchClosed() == true) { - Robot.elevator.setControlMode(DriveControlMode.STOP_MOTORS); + if (controlMode == DriveControlMode.JOYSTICK) { + Robot.elevator.setControlMode(DriveControlMode.STOP_MOTORS); + } pressed = true; } else { - Robot.elevator.setControlMode(DriveControlMode.JOYSTICK); + if (controlMode == DriveControlMode.STOP_MOTORS){ + { + Robot.elevator.setControlMode(DriveControlMode.JOYSTICK); + } + pressed = false; + } } + + } //pressed = (isPressed.isFwdLimitSwitchClosed() == true) ? true : false; - } - public double getelevatorPositionWorld() { - return elevatorRight.getPositionWorld(); - } + + @Override public void controlLoopUpdate() { @@ -344,6 +368,12 @@ public class Elevator extends Subsystem implements ControlLoopable { isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition()); } + /* + else if(controlMode == DriveControlMode.RAW) + { + isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition()); + } + */ } } @@ -355,7 +385,7 @@ public class Elevator extends Subsystem implements ControlLoopable @Override public void periodic() { - isSwitchPressed(); + // isSwitchPressed(); } @Override @@ -386,6 +416,8 @@ public class Elevator extends Subsystem implements ControlLoopable { try { + SmartDashboard.putNumber("Elevator Pos Ticks", elevatorRight.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Elevator Pos Inches", getElevatorHeightInchesAboveFloor()); //SmartDashboard.putData(pressed); } catch (Exception e) @@ -394,9 +426,7 @@ public class Elevator extends Subsystem implements ControlLoopable } } } - public void rawMoveSteer(double move, double steer) { - m_drive.arcadeDrive(move, steer, false); - } + } diff --git a/src/org/usfirst/frc4388/robot/subsystems/ElevatorAuton.java b/src/org/usfirst/frc4388/robot/subsystems/ElevatorAuton.java deleted file mode 100644 index a3c1451..0000000 --- a/src/org/usfirst/frc4388/robot/subsystems/ElevatorAuton.java +++ /dev/null @@ -1,120 +0,0 @@ - -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 org.usfirst.frc4388.robot.Robot; - -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; -import com.ctre.phoenix.motorcontrol.LimitSwitchSource; -import com.ctre.phoenix.motorcontrol.SensorCollection; - - - -/** - * - */ -public class ElevatorAuton extends Subsystem { - - private WPI_TalonSRX ElevatorRight; - private WPI_TalonSRX ElevatorLeft; - public static final double RAISE_ELEVATOR_SPEED = 1; - public static final double LOWER_ELEVATOR_SPEED = -1; - public static final double STOP_ELEVATOR_SPEED = 0; - LimitSwitchSource limitSwitchSource; - public boolean pressed; - SensorCollection isPressed; - /////^^^^^^^^^ replace this line with the modes we need - - - private boolean isFinished; - //private CarriageControlMode controlMode = CarriageControlMode.JOYSTICK; - private double PeriodMs; - - - - - public ElevatorAuton() { - try { - ElevatorLeft = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR1_ID); - ElevatorRight = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR2_ID); - ElevatorRight.setInverted(false); - ElevatorLeft.setInverted(false); - - - - - //carriageLeft.set(CurrentLimit, value); - - ElevatorRight.set(ControlMode.Follower, ElevatorLeft.getDeviceID()); - } - catch (Exception e) { - System.err.println("An error occurred in the elevator constructor"); - - - } - } - - public void setRaiseSpeed(double raiseSpeed) { - ElevatorLeft.set(-raiseSpeed); - } - - // public synchronized void setControlMode(CarriageControlMode controlMode) { - // this.controlMode = controlMode; - // if (controlMode == CarriageControlMode.JOYSTICK) { - - // carriageLeft.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving - // carriageRight.set(ControlMode.PercentOutput, 0); - // } - // } - - - - @Override - /*public void controlLoopUpdate() { - pressed = false; - isPressed = ElevatorLeft.getSensorCollection(); - - elevator_input = Robot.oi.getOperatorJoystick().getRawAxis(RobotMap.OPERATOR_JOYSTICK_1_USB_ID); - System.err.println(isPressed.isFwdLimitSwitchClosed()); - if (isPressed.isFwdLimitSwitchClosed() == true) { - ElevatorLeft.set(0); - ElevatorRight.set(0); - pressed = true; - } else { - - ElevatorLeft.set(elevator_input); - ElevatorRight.set(elevator_input); - pressed = false; - } - } - */ - - public void periodic() { - - } - - - @Override - // public void setPeriodMs(long periodMs) { - // this.PeriodMs = periodMs; - // } - public void initDefaultCommand() { - } - public void isFinished() { - } - - - - -} -