diff --git a/.classpath b/.classpath index 3ec2cb3..2a94490 100644 --- a/.classpath +++ b/.classpath @@ -12,5 +12,6 @@ + diff --git a/src/org/usfirst/frc4388/robot/Constants.java b/src/org/usfirst/frc4388/robot/Constants.java index 71e8706..93ab2c1 100644 --- a/src/org/usfirst/frc4388/robot/Constants.java +++ b/src/org/usfirst/frc4388/robot/Constants.java @@ -33,8 +33,8 @@ public class Constants { 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; + public static double kElevatorBasicPercentOutputUp = -.8; + public static double kElevatorBasicPercentOutputDown =.7; // CONTROL LOOP GAINS diff --git a/src/org/usfirst/frc4388/robot/OI.java b/src/org/usfirst/frc4388/robot/OI.java index c518792..e3a4232 100644 --- a/src/org/usfirst/frc4388/robot/OI.java +++ b/src/org/usfirst/frc4388/robot/OI.java @@ -193,7 +193,6 @@ public class OI SmartDashboard.putData("move elevator", new ElevatorBasic(20)); - SmartDashboard.putData("test", new RightSwitchAuton()); ///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 bea5105..ae18c00 100644 --- a/src/org/usfirst/frc4388/robot/Robot.java +++ b/src/org/usfirst/frc4388/robot/Robot.java @@ -127,7 +127,7 @@ public class Robot extends IterativeRobot RRautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore()); RRautonTaskChooser.addObject("Center to Right Switch", new CenterRight()); - + RRautonTaskChooser.addObject("Center to Right 2 cube", new CenterRight2Cube()); RRautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton()); RRautonTaskChooser.addObject("Right to Right Scale", new ScaleFrom3()); @@ -148,6 +148,7 @@ public class Robot extends IterativeRobot RLautonTaskChooser.addObject("Left to Left Scale", new ScaleFrom1()); RLautonTaskChooser.addObject("Center to Right Switch", new CenterRight()); + RLautonTaskChooser.addObject("Center to Right 2 cube", new CenterRight2Cube()); RLautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton()); @@ -168,6 +169,7 @@ public class Robot extends IterativeRobot LLautonTaskChooser.addObject("Center to Left Switch", new CenterLeft()); + LLautonTaskChooser.addObject("Center to Left Switch 2 cube", new CenterLeft2Cube()); LLautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore()); @@ -183,6 +185,7 @@ public class Robot extends IterativeRobot LRautonTaskChooser.addObject("Left to Left Switch", new LeftSwitchAuton()); LRautonTaskChooser.addObject("Center to Left Switch", new CenterLeft()); + LRautonTaskChooser.addObject("Center to Left Switch 2 cube", new CenterLeft2Cube()); LRautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore()); LRautonTaskChooser.addObject("Right To Right Scale", new ScaleFrom3()); diff --git a/src/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java b/src/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java index d66dd5e..a477dc7 100644 --- a/src/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java +++ b/src/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java @@ -59,7 +59,7 @@ public class DriveStraightBasic extends Command { m_lastCommandExecuteTimestamp = currentTimestamp; double steer = 0.0; if (m_useGyroLock) { - steer = - Robot.drive.getGyroAngleDeg() / 20.0; //TODO: tune + steer = - Robot.drive.getGyroAngleDeg() / 25.0; //TODO: tune } Robot.drive.rawMoveSteer(m_speed, steer); //SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java b/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java index eba9486..a6a5829 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java @@ -28,18 +28,19 @@ public class CenterLeft extends CommandGroup { addSequential(new IntakePosition(true)); addSequential(new DriveSpeedShift(true)); - addSequential(new DriveStraightBasic(-15, 50, true, true, 0)); - addSequential(new DriveTurnBasic(true, 130, 300, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasic(53, 50, true, true, 0)); - addSequential(new ElevatorBasic(20)); - addSequential(new DriveTurnBasic(true, 34.5, 300, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasic(19, 50, true, true, 0)); + addSequential(new DriveStraightBasic(-15, 30, true, true, 0)); + addSequential(new DriveTurnBasic(true, 115, 150, MPSoftwareTurnType.TANK)); + addSequential(new WaitCommand(.2)); + addSequential(new DriveStraightBasic(80, 45, true, true, 0)); + addSequential(new ElevatorBasic(30)); + addSequential(new DriveTurnBasic(true, 17, 150, MPSoftwareTurnType.TANK)); + addSequential(new DriveStraightBasic(10, 45, true, true, 0)); addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); //addSequential(new WaitCommand(.1)); addSequential(new IntakePosition(false)); addSequential(new WaitCommand(.2)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); - addSequential(new DriveStraightBasic(-20, 50, true, true, 0)); + addSequential(new DriveStraightBasic(-20, 45, true, true, 0)); addSequential(new ElevatorBasic(10)); addSequential(new DriveSpeedShift(false)); } diff --git a/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft2Cube.java b/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft2Cube.java new file mode 100644 index 0000000..1857e60 --- /dev/null +++ b/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft2Cube.java @@ -0,0 +1,65 @@ +package org.usfirst.frc4388.robot.commands.auton; + + +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.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.utility.MPSoftwarePIDController.MPSoftwareTurnType; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.WaitCommand; + +/** + * + */ +public class CenterLeft2Cube extends CommandGroup { + + public CenterLeft2Cube() + { + addSequential(new DriveGyroReset()); + addSequential(new IntakePosition(true)); + addSequential(new DriveSpeedShift(true)); + + addSequential(new DriveStraightBasic(-15, 30, true, true, 0)); + addSequential(new DriveTurnBasic(true, 115, 150, MPSoftwareTurnType.TANK)); + addSequential(new WaitCommand(.2)); + addSequential(new DriveStraightBasic(80, 45, true, true, 0)); + addSequential(new ElevatorBasic(30)); + addSequential(new DriveTurnBasic(true, 17, 150, MPSoftwareTurnType.TANK)); + addSequential(new DriveStraightBasic(10, 45, true, true, 0)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); + //addSequential(new WaitCommand(.1)); + addSequential(new IntakePosition(false)); + addSequential(new WaitCommand(.2)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); + addSequential(new DriveStraightBasic(-20, 45, true, true, 0)); + addSequential(new ElevatorBasic(10)); + addSequential(new WaitCommand(.2)); + addSequential(new ElevatorBasic(2)); + addSequential(new DriveTurnBasic(true, -90, 150, MPSoftwareTurnType.TANK)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED)); + addSequential(new DriveStraightBasic(20, 45, true, true, 0)); + addSequential(new IntakePosition(true)); + addSequential(new DriveStraightBasic(-20, 45, true, true, 0)); + addSequential(new ElevatorBasic(25)); + addSequential(new DriveTurnBasic(true, 90, 150, MPSoftwareTurnType.TANK)); + addSequential(new DriveStraightBasic(10, 45, true, true, 0)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); + //addSequential(new WaitCommand(.1)); + addSequential(new IntakePosition(false)); + addSequential(new WaitCommand(.2)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); + addSequential(new DriveStraightBasic(-20, 45, true, true, 0)); + + addSequential(new DriveSpeedShift(false)); + + } +} diff --git a/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java b/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java index 85b3476..8481821 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java @@ -22,25 +22,26 @@ import edu.wpi.first.wpilibj.command.WaitCommand; */ public class CenterRight extends CommandGroup { - public CenterRight() + public CenterRight() { addSequential(new DriveGyroReset()); addSequential(new IntakePosition(true)); addSequential(new DriveSpeedShift(true)); - - addSequential(new DriveStraightBasic(-15, 50, true, true, 0)); - addSequential(new DriveTurnBasic(true, -130, 300, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasic(53, 50, true, true, 0)); - addSequential(new ElevatorBasic(20)); - addSequential(new DriveTurnBasic(true, -34.5, 300, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasic(19, 50, true, true, 0)); + addSequential(new DriveStraightBasic(-15, 30, true, true, 0)); + addSequential(new DriveTurnBasic(true, -115, 150, MPSoftwareTurnType.TANK)); + addSequential(new WaitCommand(.2)); + addSequential(new DriveStraightBasic(80, 45, true, true, 0)); + addSequential(new ElevatorBasic(25)); + addSequential(new DriveTurnBasic(true, -35, 150, MPSoftwareTurnType.TANK)); + addSequential(new DriveStraightBasic(25, 45, true, true, 0)); addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); //addSequential(new WaitCommand(.1)); addSequential(new IntakePosition(false)); addSequential(new WaitCommand(.2)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); - addSequential(new DriveStraightBasic(-20, 50, true, true, 0)); + addSequential(new DriveStraightBasic(-25, 45, true, true, 0)); + addSequential(new ElevatorBasic(10)); addSequential(new DriveSpeedShift(false)); } } diff --git a/src/org/usfirst/frc4388/robot/commands/auton/CenterRight2Cube.java b/src/org/usfirst/frc4388/robot/commands/auton/CenterRight2Cube.java new file mode 100644 index 0000000..6de2081 --- /dev/null +++ b/src/org/usfirst/frc4388/robot/commands/auton/CenterRight2Cube.java @@ -0,0 +1,60 @@ +package org.usfirst.frc4388.robot.commands.auton; + +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.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.utility.MPSoftwarePIDController.MPSoftwareTurnType; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.WaitCommand; + +public class CenterRight2Cube extends CommandGroup { + + public CenterRight2Cube() + { + addSequential(new DriveGyroReset()); + addSequential(new IntakePosition(true)); + addSequential(new DriveSpeedShift(true)); + + addSequential(new DriveStraightBasic(-15, 30, true, true, 0)); + addSequential(new DriveTurnBasic(true, -115, 150, MPSoftwareTurnType.TANK)); + addSequential(new WaitCommand(.2)); + addSequential(new DriveStraightBasic(80, 45, true, true, 0)); + addSequential(new ElevatorBasic(25)); + addSequential(new DriveTurnBasic(true, -35, 150, MPSoftwareTurnType.TANK)); + addSequential(new DriveStraightBasic(15, 45, true, true, 0)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); + //addSequential(new WaitCommand(.1)); + addSequential(new IntakePosition(false)); + addSequential(new WaitCommand(.2)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); + addSequential(new DriveStraightBasic(-25, 45, true, true, 0)); + addSequential(new ElevatorBasic(10)); + addSequential(new WaitCommand(.2)); + addSequential(new ElevatorBasic(2)); + addSequential(new DriveTurnBasic(true, 90, 150, MPSoftwareTurnType.TANK)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED)); + addSequential(new DriveStraightBasic(20, 45, true, true, 0)); + addSequential(new IntakePosition(true)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); + addSequential(new DriveStraightBasic(-20, 45, true, true, 0)); + + addSequential(new ElevatorBasic(25)); + addSequential(new DriveTurnBasic(true, -90, 150, MPSoftwareTurnType.TANK)); + addSequential(new DriveStraightBasic(10, 45, true, true, 0)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); + //addSequential(new WaitCommand(.1)); + addSequential(new IntakePosition(false)); + addSequential(new WaitCommand(.2)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); + addSequential(new DriveStraightBasic(-20, 45, true, true, 0)); + + addSequential(new DriveSpeedShift(false)); + + } +} diff --git a/src/org/usfirst/frc4388/robot/commands/auton/LeftStartRightScore.java b/src/org/usfirst/frc4388/robot/commands/auton/LeftStartRightScore.java index b8352e4..94a81fb 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/LeftStartRightScore.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/LeftStartRightScore.java @@ -25,24 +25,30 @@ public class LeftStartRightScore extends CommandGroup { public LeftStartRightScore() { addSequential(new DriveGyroReset()); + addSequential(new WaitCommand(.1)); addSequential(new DriveSpeedShift(true)); addSequential(new IntakePosition(true)); - addSequential(new DriveStraightBasic(215, 50, true, true, 0)); + addSequential(new DriveStraightBasic(-215, 40, true, true, 0)); + addSequential(new WaitCommand(.2)); addSequential(new ElevatorBasic(5)); - addSequential(new DriveTurnBasic(true, -87, 300, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasic(180, 50, true, true, 0)); + addSequential(new DriveTurnBasic(true, -73, 150, MPSoftwareTurnType.TANK)); + addSequential(new WaitCommand(.2)); + addSequential(new DriveStraightBasic(155, 60, true, true, 0)); addSequential(new ElevatorBasic(30)); - addSequential(new DriveTurnBasic(true, 90, 300, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasic(5, 40, true, true, 0)); + addSequential(new WaitCommand(.2)); + addSequential(new DriveTurnBasic(true, 73, 150, MPSoftwareTurnType.TANK)); + addSequential(new WaitCommand(.2)); + addSequential(new DriveStraightBasic(10, 60, true, true, 0)); addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); //addSequential(new WaitCommand(.1)); addSequential(new IntakePosition(false)); + addSequential(new DriveStraightBasic(5, 60, true, true, 0)); addSequential(new WaitCommand(.2)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); - addSequential(new DriveStraightBasic(-10, 50, true, true, 0)); + addSequential(new DriveStraightBasic(-15, 60, true, true, 0)); addSequential(new DriveSpeedShift(false)); - //addSequential(new DriveStraightMP(50, 10, true, true, 0)); // 95 for 112" greenville + } } diff --git a/src/org/usfirst/frc4388/robot/commands/auton/LeftSwitchAuton.java b/src/org/usfirst/frc4388/robot/commands/auton/LeftSwitchAuton.java index 551db80..3e8d35c 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/LeftSwitchAuton.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/LeftSwitchAuton.java @@ -30,7 +30,7 @@ public class LeftSwitchAuton extends CommandGroup { addSequential(new DriveStraightBasic(-130, 50, true, true, 0)); addSequential(new ElevatorBasic(30)); - addSequential(new DriveTurnBasic(true, -87, 300, MPSoftwareTurnType.TANK)); + addSequential(new DriveTurnBasic(true, -87, 150, MPSoftwareTurnType.TANK)); addSequential(new ElevatorBasic(30)); addSequential(new DriveStraightBasic(20, 50, true, true, 0)); addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java b/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java index 9063322..6759ddd 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java @@ -25,24 +25,30 @@ public class RightStartLeftScore extends CommandGroup { public RightStartLeftScore() //////////TUNEDISH AND ALMOST GOOD { addSequential(new DriveGyroReset()); + addSequential(new WaitCommand(.1)); addSequential(new DriveSpeedShift(true)); addSequential(new IntakePosition(true)); - addSequential(new DriveStraightBasic(-215, 50, true, true, 0)); + addSequential(new DriveStraightBasic(-215, 40, true, true, 0)); + addSequential(new WaitCommand(.2)); addSequential(new ElevatorBasic(5)); - addSequential(new DriveTurnBasic(true, 87, 300, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasic(180, 50, true, true, 0)); + addSequential(new DriveTurnBasic(true, 73, 150, MPSoftwareTurnType.TANK)); + addSequential(new WaitCommand(.2)); + addSequential(new DriveStraightBasic(155, 60, true, true, 0)); addSequential(new ElevatorBasic(30)); - addSequential(new DriveTurnBasic(true, -90, 300, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasic(10, 50, true, true, 0)); + addSequential(new WaitCommand(.2)); + addSequential(new DriveTurnBasic(true, -73, 150, MPSoftwareTurnType.TANK)); + addSequential(new WaitCommand(.2)); + addSequential(new DriveStraightBasic(10, 60, true, true, 0)); addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); //addSequential(new WaitCommand(.1)); addSequential(new IntakePosition(false)); + addSequential(new DriveStraightBasic(5, 60, true, true, 0)); addSequential(new WaitCommand(.2)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); - addSequential(new DriveStraightBasic(-10, 50, true, true, 0)); + addSequential(new DriveStraightBasic(-15, 60, true, true, 0)); addSequential(new DriveSpeedShift(false)); - //addSequential(new DriveStraightMP(50, 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 93e53b3..71a7c8b 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/RightSwitchAuton.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/RightSwitchAuton.java @@ -28,11 +28,12 @@ public class RightSwitchAuton extends CommandGroup { addSequential(new IntakePosition(true)); - addSequential(new DriveStraightBasic(-130, 50, true, true, 0)); + addSequential(new DriveStraightBasic(-140, 50, true, true, 0)); addSequential(new ElevatorBasic(30)); - addSequential(new DriveTurnBasic(true, 87, 300, MPSoftwareTurnType.TANK)); + addSequential(new WaitCommand(.2)); + addSequential(new DriveTurnBasic(true, 87, 150, MPSoftwareTurnType.TANK)); addSequential(new ElevatorBasic(30)); - addSequential(new DriveStraightBasic(20, 50, true, true, 0)); + addSequential(new DriveStraightBasic(8, 50, 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/commands/auton/ScaleFrom1.java b/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom1.java index 5765336..47030f4 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom1.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom1.java @@ -37,8 +37,9 @@ public class ScaleFrom1 extends CommandGroup { addSequential(new IntakePosition(false)); addSequential(new WaitCommand(.5)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); - addSequential(new DriveStraightBasic(-15, 20, true, true, 0)); + addSequential(new DriveStraightBasic(-25, 20, true, true, 0)); addSequential(new ElevatorBasic(10)); + addSequential(new DriveSpeedShift(false)); //addSequential(new DriveStraightMP(50, 10, true, true, 0)); // 95 for 112" greenville diff --git a/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java b/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java index 60e05b0..a57c377 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java @@ -28,20 +28,24 @@ public class ScaleFrom3 extends CommandGroup { addSequential(new IntakePosition(true)); - addSequential(new DriveStraightBasic(-290, 50, true, true, 0)); + addSequential(new DriveStraightBasic(-10, 45, true, true, 0)); + //addSequential(new DriveTurnBasic(true, -2, 100, MPSoftwareTurnType.TANK)); + addSequential(new DriveStraightBasic(-270, 45, true, true, 0)); addSequential(new ElevatorBasic(70)); + addSequential(new DriveStraightBasic(-30, 20, true, true, 0)); addSequential(new DriveTurnBasic(true, 90, 100, MPSoftwareTurnType.TANK)); //addSequential(new DriveStraightBasic(5, 20, true, true, 0)); + addSequential(new WaitCommand(.5)); addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); //addSequential(new WaitCommand(.1)); addSequential(new IntakePosition(false)); addSequential(new WaitCommand(.5)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); - addSequential(new DriveStraightBasic(-15, 20, true, true, 0)); + addSequential(new DriveStraightBasic(-25, 20, true, true, 0)); addSequential(new ElevatorBasic(10)); addSequential(new DriveSpeedShift(false)); - //addSequential(new DriveStraightMP(50, 10, true, true, 0)); // 95 for 112" greenville + //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/subsystems/Carriage.java b/src/org/usfirst/frc4388/robot/subsystems/Carriage.java index 6cf70b0..bde3755 100644 --- a/src/org/usfirst/frc4388/robot/subsystems/Carriage.java +++ b/src/org/usfirst/frc4388/robot/subsystems/Carriage.java @@ -35,7 +35,7 @@ public class Carriage extends Subsystem { private WPI_TalonSRX carriageRight; public static enum CarriageControlMode { JOYSTICK, MP_STRAIGHT, HOLD, MANUAL}; public static final double CUBE_INTAKE_SPEED = 0.40; - public static final double CUBE_EJECT_SPEED = -1.0; + public static final double CUBE_EJECT_SPEED = -0.8; public static final double CUBE_STOP_SPEED = 0; /////^^^^^^^^^ replace this line with the modes we need diff --git a/src/org/usfirst/frc4388/robot/subsystems/Elevator.java b/src/org/usfirst/frc4388/robot/subsystems/Elevator.java index 5eba8fd..4a17e98 100644 --- a/src/org/usfirst/frc4388/robot/subsystems/Elevator.java +++ b/src/org/usfirst/frc4388/robot/subsystems/Elevator.java @@ -25,6 +25,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; +import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; import com.ctre.phoenix.motorcontrol.LimitSwitchSource; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.SensorCollection; @@ -100,14 +101,34 @@ public class Elevator extends Subsystem implements ControlLoopable //Setting left elevator motor as follower elevatorLeft.set(ControlMode.Follower, elevatorRight.getDeviceID()); elevatorLeft.setInverted(false); - elevatorRight.setSensorPhase(true); elevatorLeft.setNeutralMode(NeutralMode.Brake); elevatorRight.setNeutralMode(NeutralMode.Brake); + elevatorRight.setSensorPhase(true); //Limit Switch Left //elevatorLeft.overrideLimitSwitchesEnable(true); elevatorLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); elevatorLeft.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + //Limit Switch Right + //elevatorRight.overrideLimitSwitchesEnable(true); + //elevatorRight.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + //elevatorRight.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + + + //Change This boi + + // elevatorRight.configForwardSoftLimitThreshold(10000, 0); //right here + //elevatorRight.configReverseSoftLimitThreshold(5, 0); + //elevatorRight.configForwardSoftLimitEnable(true, 0); + //elevatorRight.configReverseSoftLimitEnable(true, 0); + + //sos + //elevatorRight.enableLimitSwitch(true, true); + + + + + } catch(Exception e) { @@ -211,14 +232,15 @@ public class Elevator extends Subsystem implements ControlLoopable { elevatorRight.set(moveElevatorInput); } - /* + + /* if(elevatorPos <= elevatorSafeZone && elevatorPos >= 0) { elevatorRight.set(moveElevatorInput); } else if(elevatorPos > elevatorSafeZone) { - elevatorRight.set(moveElevatorInput * 0.8); + elevatorRight.set(moveElevatorInput * 0.65); if(holdButtonPressed == true) @@ -229,17 +251,19 @@ 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 - } + //PID encoder position public double getEncoderElevatorPosition() @@ -295,7 +319,7 @@ public class Elevator extends Subsystem implements ControlLoopable elevatorRight.set(/*ControlMode.PercentOutput,*/ output); } - /*public void holdInPos() + public void holdInPos() { elevatorRight.set(-0.43 * 0.2); } @@ -326,9 +350,9 @@ public class Elevator extends Subsystem implements ControlLoopable pressed = false; } - }*/ + } - + } //pressed = (isPressed.isFwdLimitSwitchClosed() == true) ? true : false; diff --git a/src/org/usfirst/frc4388/utility/CANTalonEncoder.java b/src/org/usfirst/frc4388/utility/CANTalonEncoder.java index 7217bf7..52088f7 100644 --- a/src/org/usfirst/frc4388/utility/CANTalonEncoder.java +++ b/src/org/usfirst/frc4388/utility/CANTalonEncoder.java @@ -8,6 +8,8 @@ public class CANTalonEncoder extends WPI_TalonSRX private double encoderTicksToWorld; private boolean isRight = true; + + public CANTalonEncoder(int deviceNumber, double encoderTicksToWorld, FeedbackDevice feedbackDevice) { this(deviceNumber, encoderTicksToWorld, false, feedbackDevice); } @@ -60,4 +62,4 @@ public class CANTalonEncoder extends WPI_TalonSRX //return convertEncoderTicksToWorld(this.getSpeed() / 0.1); return convertEncoderTicksToWorld(this.getSelectedSensorVelocity(0) / 0.1); //TODO: verify want 0="Primary closed-loop" } -} \ No newline at end of file +}