diff --git a/src/org/robockets/robot/OI.java b/src/org/robockets/robot/OI.java index 8c1bc98..0cee6e8 100644 --- a/src/org/robockets/robot/OI.java +++ b/src/org/robockets/robot/OI.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj.buttons.JoystickButton; import edu.wpi.first.wpilibj.command.Command; import org.robockets.commons.RelativeDirection; +import org.robockets.robot.climber.Climb; import org.robockets.robot.cubeintake.IntakeCube; import org.robockets.robot.cubeintake.IntakeOut; import org.robockets.robot.drivetrain.FakeDriveAssisted; @@ -31,6 +32,7 @@ public class OI { public static Button driverA = new JoystickButton(driverJoystick, 1); public static Button driverB = new JoystickButton(driverJoystick, 2); + public static Button driverY = new JoystickButton(driverJoystick, 4); public static Button driverSelect = new JoystickButton(driverJoystick, 7); public static Button driverStart = new JoystickButton(driverJoystick, 8); @@ -43,17 +45,18 @@ public class OI { public static Button operatorRightStick = new JoystickButton(operatorJoystick, 10); public OI() { - driverA.whileHeld(new IntakeCube(0.75, 0.4, 0.5)); + driverA.whileHeld(new IntakeCube(0.9, 0.6, 0.8)); driverB.whileHeld(new IntakeOut(0.75, 0.4)); + driverY.whileHeld(new Climb(0.75)); driverSelect.whenPressed(new Joyride()); driverStart.whenPressed(new FakeDriveAssisted()); - operatorA.whileHeld(new IntakeCube(0.75, 0.4, 0.65)); - operatorB.whenPressed(new Elevate(ElevatorPosition.BOTTOM, 0.5)); - operatorX.whenPressed(new Elevate(ElevatorPosition.EXCHANGE, 0.5)); - operatorY.whenPressed(new Elevate(ElevatorPosition.SWITCH, 0.5)); + operatorA.whileHeld(new IntakeCube(0.9, 0.6, 0.8)); + //operatorB.whenPressed(new Elevate(ElevatorPosition.BOTTOM, 0.5)); + //operatorX.whenPressed(new Elevate(ElevatorPosition.EXCHANGE, 0.5)); + //operatorY.whenPressed(new Elevate(ElevatorPosition.SWITCH, 0.5)); operatorSelect.whenPressed(new InterruptElevator()); - operatorLeftStick.whenPressed(new Elevate(ElevatorPosition.MID_SCALE, 0.75)); + //operatorLeftStick.whenPressed(new Elevate(ElevatorPosition.MID_SCALE, 0.75)); //operatorRightStick.whenPressed(new Elevate(ElevatorPosition.HIGH_SCALE, 0.75)); } } diff --git a/src/org/robockets/robot/Robot.java b/src/org/robockets/robot/Robot.java index 6d4bca0..5d3c918 100644 --- a/src/org/robockets/robot/Robot.java +++ b/src/org/robockets/robot/Robot.java @@ -20,6 +20,7 @@ import org.robockets.robot.drivetrain.ResetEncoders; import org.robockets.robot.drivetrain.StartGyroPID; import org.robockets.robot.elevator.Elevator; +import org.robockets.robot.elevator.ElevatorCounter; import org.robockets.robot.elevator.ElevatorFloor; import org.robockets.robot.climber.Climber; import org.robockets.robot.autonomous.AutoChooser; @@ -51,6 +52,7 @@ public class Robot extends TimedRobot { public static Command intakeListener; public static Command elevatorFloorListener; public static Command manualElevate;*/ + public static Command elevatorCounter; public static Elevator elevator; @@ -73,17 +75,21 @@ public void robotInit() { RobotMap.gyro.reset(); RobotMap.elevatorMotor.getSensorCollection().setPulseWidthPosition(0, 0); + RobotMap.leftEncoder.setDistancePerPulse(1.0 / 39.92); + RobotMap.rightEncoder.setDistancePerPulse(1.0 / 39.92); + drivetrain = new Drivetrain(); cubeIntake = new CubeIntake(); elevator = new Elevator(); elevatorFloor = new ElevatorFloor(); + elevatorCounter = new ElevatorCounter(); + RobotMap.elevatorMotor.setInverted(true); + RobotMap.elevatorFloorMotor.setInverted(true); RobotMap.elevatorFloorMotor2.setInverted(true); - - RobotMap.leftEncoder.setDistancePerPulse(1 / 39.92); - RobotMap.rightEncoder.setDistancePerPulse(1 / 39.92); RobotMap.rightEncoder.setReverseDirection(true); + RobotMap.leftDrivepodSpeedController.setInverted(true); RobotMap.rightDrivepodSpeedController.setInverted(true); @@ -93,15 +99,10 @@ public void robotInit() { SmartDashboard.putNumber("Gyro Setpoint", 0); SmartDashboard.putData(new StartGyroPID());*/ - SmartDashboard.putNumber("Left Encoder P", drivetrain.leftPodPID.getP()); - SmartDashboard.putNumber("Left Encoder I", drivetrain.leftPodPID.getI()); - SmartDashboard.putNumber("Left Encoder D", drivetrain.leftPodPID.getD()); - SmartDashboard.putNumber("Left Encoder Setpoint", 0); - - SmartDashboard.putNumber("Right Encoder P", drivetrain.rightPodPID.getP()); - SmartDashboard.putNumber("Right Encoder I", drivetrain.rightPodPID.getI()); - SmartDashboard.putNumber("Right Encoder D", drivetrain.rightPodPID.getD()); - SmartDashboard.putNumber("Right Encoder Setpoint", 0); + SmartDashboard.putNumber("Encoder P", drivetrain.encoderPID.getP()); + SmartDashboard.putNumber("Encoder I", drivetrain.encoderPID.getI()); + SmartDashboard.putNumber("Encoder D", drivetrain.encoderPID.getD()); + SmartDashboard.putNumber("Encoder Setpoint", 0); SmartDashboard.putData(new StartEncoderPID()); autoChooser.addObject("Test Auto", AutoHelper.AutoType.TEST); @@ -128,6 +129,8 @@ public void robotInit() { SmartDashboard.putData("Reset Encoders", new ResetEncoders()); + elevatorCounter.start(); + m_oi = new OI(); } @@ -141,8 +144,7 @@ public void robotPeriodic() { SmartDashboard.putNumber("Left Distance", RobotMap.leftEncoder.getDistance()); SmartDashboard.putNumber("Right Distance", RobotMap.rightEncoder.getDistance()); - SmartDashboard.putNumber("Left PID Output", drivetrain.leftPodPID.get()); - SmartDashboard.putNumber("Right PID Output", drivetrain.rightPodPID.get()); + SmartDashboard.putNumber("Encoder PID Output", drivetrain.encoderPID.get()); /*SmartDashboard.putNumber("PDP0: ", RobotMap.pdp.getCurrent(0)); SmartDashboard.putNumber("PDP1: ", RobotMap.pdp.getCurrent(1)); SmartDashboard.putNumber("PDP14: ", RobotMap.pdp.getCurrent(14)); diff --git a/src/org/robockets/robot/RobotMap.java b/src/org/robockets/robot/RobotMap.java index 36fdc0b..6cc9c52 100644 --- a/src/org/robockets/robot/RobotMap.java +++ b/src/org/robockets/robot/RobotMap.java @@ -35,20 +35,17 @@ public class RobotMap { public static Victor leftIntake = new Victor(4); // STROUT!!!! public static Victor barIntake = new Victor(5); - public static Victor climberMotorLeft = new Victor(7); - public static Victor climberMotorRight = new Victor(3); + public static Victor climberMotor = new Victor(7); public static DifferentialDrive robotDrive = new DifferentialDrive(leftDrivepodSpeedController, rightDrivepodSpeedController); public static ADXRS450_Gyro gyro = new ADXRS450_Gyro(SPI.Port.kOnboardCS0); - public static Encoder leftEncoder = new Encoder(0, 1); + public static Encoder leftEncoder = new Encoder(8, 9); public static Encoder rightEncoder = new Encoder(2, 3); public static PowerDistributionPanel pdp = new PowerDistributionPanel(0); // Zero is the CAN id of the powerdistribution panel. public static int climberMotorPDPChannel = 1; // Dummy value. - public static DigitalInput elevatorEncoder = new DigitalInput(5); - - public static Counter counter = new Counter(elevatorEncoder); + public static DigitalInput elevatorEncoder = new DigitalInput(4); } diff --git a/src/org/robockets/robot/autonomous/AutoChooser.java b/src/org/robockets/robot/autonomous/AutoChooser.java index b0fdcc3..21eaf5e 100644 --- a/src/org/robockets/robot/autonomous/AutoChooser.java +++ b/src/org/robockets/robot/autonomous/AutoChooser.java @@ -12,7 +12,7 @@ public AutoChooser(AutoHelper.AutoType auto, AutoHelper.RobotPosition startingPo Command autoCommand = new DumbAuto(); switch (auto) { case TEST: - autoCommand = new TestAuto2(); + autoCommand = new TestAuto(); break; case DUMB: autoCommand = new DumbAuto(); diff --git a/src/org/robockets/robot/autonomous/DumbAuto.java b/src/org/robockets/robot/autonomous/DumbAuto.java index 6efb854..ec3139b 100644 --- a/src/org/robockets/robot/autonomous/DumbAuto.java +++ b/src/org/robockets/robot/autonomous/DumbAuto.java @@ -9,6 +9,6 @@ public class DumbAuto extends CommandGroup { public DumbAuto() { - addSequential(new DriveStraight(15, .5)); // TODO: Change this to a correct number + addSequential(new DriveStraight(10, .5)); // TODO: Change this to a correct number } } diff --git a/src/org/robockets/robot/autonomous/MidAuto.java b/src/org/robockets/robot/autonomous/MidAuto.java index 803e5f7..9b840f2 100644 --- a/src/org/robockets/robot/autonomous/MidAuto.java +++ b/src/org/robockets/robot/autonomous/MidAuto.java @@ -5,12 +5,14 @@ import edu.wpi.first.wpilibj.command.WaitCommand; import org.robockets.robot.Robot; import org.robockets.robot.cubeintake.DropCube; +import org.robockets.robot.cubeintake.IntakeCube; import org.robockets.robot.drivetrain.DriveAngleAssisted; import org.robockets.robot.drivetrain.DriveStraight; import org.robockets.robot.drivetrain.DriveStraightAssisted; import org.robockets.robot.drivetrain.TurnAbsolute; import org.robockets.robot.elevator.Elevate; import org.robockets.robot.elevator.ElevatorPosition; +import org.robockets.robot.elevator.TimedElevator; /** * @author Mathias Kools, Jake Backer @@ -33,7 +35,7 @@ public MidAuto(AutoHelper.RobotPosition robotPosition, AutoHelper.Priority prior switch (robotPosition) { case MIDDLE: // If in position 2 (middle) // Drive to our switch position - dropCubeMiddleToSwitch(teamSwitchLeft); + dropCubeMiddleToSwitch(teamSwitchLeft); // Tested break; case LEFT: // Else if in position 1 (left) or 3 (right) case RIGHT: // These two enums are just here for readability & changeability. @@ -42,46 +44,51 @@ public MidAuto(AutoHelper.RobotPosition robotPosition, AutoHelper.Priority prior case SCALE: //If priority is scale. if (scaleSameSide) { // If scale is on our side // Deposit cube in scale - dropCubeInSameSideScale(teamSwitchLeft); + dropCubeInSameSideScale(scaleLeft); // TODO: Test } else { // scale is on other side if (teamSwitchSameSide == false) { // If switch is on other side // Drive in S shape to other side - dropCubeInOppositeSideScaleSShape(teamSwitchLeft); + dropCubeInOppositeSideScaleSShape(teamSwitchLeft); // TODO: Test } else { // If switch is on our side // We can just drop in switch or still drive in s + dropCubeInSameSideScale(scaleLeft); } } break; case SWITCH: // Else If priority is switch if (teamSwitchSameSide) { // If switch is on our side // Deposit cube in switch. - dropCubeInSameSideSwitch(teamSwitchLeft); + dropCubeInSameSideSwitch(teamSwitchLeft); // Tested } else { // Else // If scale is on our side if (teamSwitchSameSide) { // Deposit cube in scale - dropCubeInSameSideScale(teamSwitchLeft); + dropCubeInSameSideScale(scaleLeft); // TODO: Test } else { // Else // Drive to auto line. - autoLine(); + autoLine(); // Tested } } break; default: // Else - if (scaleSameSide) { // If scale is on our side - // Deposit cube in scale - dropCubeInSameSideScale(teamSwitchLeft); - } else if (teamSwitchSameSide) { // Else if switch is on our side + if (teamSwitchSameSide) { // If switch is on our side // Deposit cube in switch - dropCubeInSameSideSwitch(teamSwitchLeft); + dropCubeInSameSideSwitch(teamSwitchLeft); // Tested + } else if (scaleSameSide) { // Else if scale is on our side + /*if(teamSwitchSameSide) { + twoCubeSS(teamSwitchLeft); + } else {*/ + // Deposit cube in scale + dropCubeInSameSideScale(scaleLeft); // TODO: Test + //} } else { // Else // Drive to auto line. - autoLine(); + autoLine(); // Tested } } } } else { - autoLine(); + autoLine(); // Tested } } @@ -100,6 +107,24 @@ private void dropCube() { addSequential(new DropCube()); } + private void twoCubeSS(boolean scaleLeft) { + addParallel(new Elevate(ElevatorPosition.MID_SCALE)); + dropCubeInSameSideScale(scaleLeft); + addSequential(new TurnAbsolute(0)); + driveStraight(-195); // Either this + //driveStraight(150); // or this + addParallel(new Elevate(ElevatorPosition.BOTTOM)); + turnAngle(135); + addParallel(new IntakeCube(0.75, 0.4, 0.65, 4)); + driveStraight(50); // TODO: Make this an actual value + driveStraight(-50); // TODO: This may or may not work + addSequential(new TurnAbsolute(0)); + driveStraight(-10); // TODO: This will change + addParallel(new Elevate(ElevatorPosition.SWITCH)); + turnAngle(90); + driveStraight(50); + } + // Small bit of repeated code that could be put into a method. // Starting position has the corner of robot right where slant of corner of field starts. private void dropCubeInSameSideSwitch(boolean teamSwitchLeft) { @@ -107,9 +132,10 @@ private void dropCubeInSameSideSwitch(boolean teamSwitchLeft) { System.out.println("SSS"); // Deposit cube in switch - driveStraight(140); + driveStraight(130); // FIXME: This will be 140 // 90deg CW if on the left. - turnAngle(teamSwitchLeft ? -90 : 90); + addSequential(new TurnAbsolute(teamSwitchLeft ? -90 : 90)); + addSequential(new WaitCommand(0.5)); // align edge of robot to edge of switch. driveStraight(16.56); //dropCube(); @@ -123,16 +149,16 @@ private void autoLine() { // Small bit of repeated code for going to the same side scale and dropping a cube. // Starting position the same as dropCubeInSameSideSwitch. - private void dropCubeInSameSideScale(boolean teamSwitchLeft) { - addParallel(new Elevate(ElevatorPosition.MID_SCALE)); + private void dropCubeInSameSideScale(boolean scaleLeft) { + //addParallel(new Elevate(ElevatorPosition.MID_SCALE)); // Deposit cube in scale. // Drive straight the distance to the scale. driveStraight(305.15); // 90deg CW if on the left. - turnAngle(teamSwitchLeft ? 90 : -90); + turnAngle(scaleLeft ? 90 : -90); driveStraight(2.88); - dropCube(); + //dropCube(); } // Code that is so ugly it should be tucked away here. FIXME: This is still super broken @@ -143,7 +169,19 @@ private void dropCubeInOppositeSideScaleSShape(boolean teamSwitchLeft) { double angle = teamSwitchLeft ? 90 : -90; turnAngle(angle); // turn 90deg CW when on the left. driveStraight(244.5); - turnAngle(-angle); // 90deg CCW when starting from the left. + turnAngle(-angle); // 90deg CCW whaddParallel(new Elevate(ElevatorPosition.SWITCH)); + + driveStraight(36); // 50 of all the numbers was chosen arbitrarily. + addSequential(new WaitCommand(0.1)); + double smallAngle = (teamSwitchLeft ? 40 : -42.5); // CW first on the left. TODO: Change these + //turnAngle(smallAngle); + addSequential(new TurnAbsolute(smallAngle)); + addSequential(new WaitCommand(0.1)); + driveStraight(68); + addSequential(new WaitCommand(0.1)); + //turnAngle(-smallAngle); + addSequential(new TurnAbsolute(0)); + addSequential(new WaitCommand(0.1)); //en starting from the left. driveStraight(98.915); turnAngle(-angle); driveStraight(2.88); @@ -154,20 +192,20 @@ private void dropCubeInOppositeSideScaleSShape(boolean teamSwitchLeft) { // of the exchange to the side of the robot. private void dropCubeMiddleToSwitch(boolean teamSwitchLeft) { //addParallel(new Elevate(ElevatorPosition.SWITCH)); + //addParallel(new TimedElevator(3.75, 0.8)); - driveStraight(36); // 50 of all the numbers was chosen arbitrarily. - addSequential(new WaitCommand(0.1)); - double smallAngle = (teamSwitchLeft ? 41.7 : -41.7); // CW first on the left. + driveStraight(25); // 50 of all the numbers was chosen arbitrarily. + + double smallAngle = (teamSwitchLeft ? 42.5 : -40.5); // CW first on the left. //turnAngle(smallAngle); addSequential(new TurnAbsolute(smallAngle)); - addSequential(new WaitCommand(0.1)); - driveStraight(68); - addSequential(new WaitCommand(0.1)); + + driveStraight(teamSwitchLeft ? 90 : 70); + //turnAngle(-smallAngle); addSequential(new TurnAbsolute(0)); - addSequential(new WaitCommand(0.1)); - driveStraight(8); - //dropCube(); + driveStraight(21); + //dropCube(); } } diff --git a/src/org/robockets/robot/autonomous/TestAuto.java b/src/org/robockets/robot/autonomous/TestAuto.java index 058db0b..97fbe09 100644 --- a/src/org/robockets/robot/autonomous/TestAuto.java +++ b/src/org/robockets/robot/autonomous/TestAuto.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj.command.WaitCommand; import org.robockets.robot.drivetrain.DriveAngleAssisted; import org.robockets.robot.drivetrain.DriveStraightAssisted; +import org.robockets.robot.drivetrain.TurnAbsolute; /** * @author Jake Backer @@ -11,8 +12,8 @@ public class TestAuto extends CommandGroup { public TestAuto() { - addSequential(new DriveStraightAssisted(36)); - addSequential(new WaitCommand(0.2)); - addSequential(new DriveStraightAssisted(-36)); + addSequential(new DriveStraightAssisted(120)); + addSequential(new TurnAbsolute(90)); + addSequential(new DriveStraightAssisted(36)); // BE CAREFUL WITH THIS } } diff --git a/src/org/robockets/robot/climber/Climb.java b/src/org/robockets/robot/climber/Climb.java new file mode 100644 index 0000000..75063f2 --- /dev/null +++ b/src/org/robockets/robot/climber/Climb.java @@ -0,0 +1,37 @@ +package org.robockets.robot.climber; + +import edu.wpi.first.wpilibj.command.Command; +import org.robockets.robot.Robot; + +/** + * @author jake + */ +public class Climb extends Command { + + private double speed; + + public Climb(double speed) { + this.speed = speed; + } + + protected void initialize() { + } + + protected void execute() { + if (Robot.climber.isStalling()) { + Robot.climber.setSpeed(speed); + } + } + + protected boolean isFinished() { + return false; + } + + protected void end() { + Robot.climber.stop(); + } + + protected void interrupted() { + end(); + } +} diff --git a/src/org/robockets/robot/climber/Climber.java b/src/org/robockets/robot/climber/Climber.java index 54d57e2..1ec1fb5 100644 --- a/src/org/robockets/robot/climber/Climber.java +++ b/src/org/robockets/robot/climber/Climber.java @@ -36,8 +36,7 @@ public double getClimberMotorCurrent() { * Stop / brake the climbing motor(s). */ public void stop() { - RobotMap.climberMotorLeft.set(0); - RobotMap.climberMotorRight.set(0); + RobotMap.climberMotor.set(0); } /* @@ -45,13 +44,7 @@ public void stop() { * @param speed The exact speed to use with the climber motor. */ public void setSpeed(double speed) { - RobotMap.climberMotorLeft.set(speed); - RobotMap.climberMotorRight.set(speed); - } - - public void setSpeed(double leftSpeed, double rightSpeed) { - RobotMap.climberMotorLeft.set(leftSpeed); - RobotMap.climberMotorRight.set(rightSpeed); + RobotMap.climberMotor.set(speed); } // SmartDashboard related commands. diff --git a/src/org/robockets/robot/climber/ClimberListener.java b/src/org/robockets/robot/climber/ClimberListener.java deleted file mode 100644 index 6e82597..0000000 --- a/src/org/robockets/robot/climber/ClimberListener.java +++ /dev/null @@ -1,53 +0,0 @@ -package org.robockets.robot.climber; - -import org.robockets.commons.RelativeDirection; -import org.robockets.robot.OI; -import org.robockets.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -/** - * This command will climberListener the scale given certain starting parameters. - */ -public class ClimberListener extends Command { - - /* - * ClimberListener until the robot climber motor stalls. - */ - public ClimberListener() { - requires(Robot.climber); - } - - // Called just before this Command runs the first time - protected void initialize() { - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - double leftSide = OI.driverJoystick.getRawAxis(2); - double rightSide = OI.driverJoystick.getRawAxis(3); - - leftSide *= OI.driverJoystick.getRawButton(5) ? -1 : 1; - rightSide *= OI.driverJoystick.getRawButton(6) ? -1 : 1; - - if (!Robot.climber.isStalling()) { - Robot.climber.setSpeed(leftSide, rightSide); - } - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return false; - } - - // Called once after isFinished returns true - protected void end() { - Robot.climber.stop(); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - end(); - } -} diff --git a/src/org/robockets/robot/cubeintake/DropCube.java b/src/org/robockets/robot/cubeintake/DropCube.java index 09bd66d..6238857 100644 --- a/src/org/robockets/robot/cubeintake/DropCube.java +++ b/src/org/robockets/robot/cubeintake/DropCube.java @@ -10,6 +10,6 @@ */ public class DropCube extends CommandGroup { public DropCube() { - addParallel(new MoveElevatorFloor(0.75, 1)); + addParallel(new MoveElevatorFloor(-0.75, 1)); } } diff --git a/src/org/robockets/robot/cubeintake/IntakeCube.java b/src/org/robockets/robot/cubeintake/IntakeCube.java index 43fc75b..8ce947d 100644 --- a/src/org/robockets/robot/cubeintake/IntakeCube.java +++ b/src/org/robockets/robot/cubeintake/IntakeCube.java @@ -21,4 +21,15 @@ public IntakeCube(double intakeSpeed, double barSpeed, double floorSpeed) { addParallel(new MoveBar(barSpeed)); addParallel(new MoveElevatorFloor(floorSpeed)); } + + public IntakeCube(double intakeSpeed, double barSpeed, double floorSpeed, double time) { + requires(Robot.cubeIntake); + requires(Robot.elevatorFloor); + + //addSequential(new Elevate(ElevatorPosition.BOTTOM, intakeSpeed)); + addParallel(new MoveBothArms(intakeSpeed, time)); + addParallel(new MoveBar(barSpeed, time)); + addParallel(new MoveElevatorFloor(floorSpeed, time)); + } + } diff --git a/src/org/robockets/robot/drivetrain/DriveAngleAssisted.java b/src/org/robockets/robot/drivetrain/DriveAngleAssisted.java index 8153d2b..4389b1b 100644 --- a/src/org/robockets/robot/drivetrain/DriveAngleAssisted.java +++ b/src/org/robockets/robot/drivetrain/DriveAngleAssisted.java @@ -27,7 +27,7 @@ protected void initialize() { } protected void execute() { - // Stub. + drivetrain.driveArcade(0, drivetrain.gyroPID.get()); } protected boolean isFinished() { diff --git a/src/org/robockets/robot/drivetrain/DriveStraightAssisted.java b/src/org/robockets/robot/drivetrain/DriveStraightAssisted.java index 569bbaa..fa08f75 100644 --- a/src/org/robockets/robot/drivetrain/DriveStraightAssisted.java +++ b/src/org/robockets/robot/drivetrain/DriveStraightAssisted.java @@ -17,18 +17,22 @@ public DriveStraightAssisted(double distance) { } protected void initialize() { - double leftPosition = RobotMap.leftEncoder.getDistance() + distance; // Convert relative to absolute - double rightPosition = RobotMap.rightEncoder.getDistance() + distance; + double position = Robot.drivetrain.getEncoderPos() + distance; // Convert relative to absolute TODO: This may need to change - Robot.drivetrain.setDistance(leftPosition, rightPosition); + Robot.drivetrain.setDistance(position); Robot.drivetrain.enableEncoderPID(); } protected void execute() { + double angle = Robot.drivetrain.getGyroPos(); + double translate = -Robot.drivetrain.encoderPID.get(); + double rotate = (angle-Robot.drivetrain.gyroPID.getSetpoint())*Robot.drivetrain.ENCODER_KP; + + Robot.drivetrain.driveArcade(translate, rotate); } protected boolean isFinished() { - return Robot.drivetrain.leftPodPID.onTarget() && Robot.drivetrain.rightPodPID.onTarget(); + return Robot.drivetrain.encoderPID.onTarget(); } protected void end() { diff --git a/src/org/robockets/robot/drivetrain/Drivetrain.java b/src/org/robockets/robot/drivetrain/Drivetrain.java index 6317d2e..638d18f 100644 --- a/src/org/robockets/robot/drivetrain/Drivetrain.java +++ b/src/org/robockets/robot/drivetrain/Drivetrain.java @@ -2,8 +2,8 @@ import org.robockets.commons.RelativeDirection; import org.robockets.robot.RobotMap; -import org.robockets.robot.pidoutput.DrivePodPIDOutput; import org.robockets.robot.pidoutput.GyroPIDOutput; +import org.robockets.robot.pidsources.DummyPIDOutput; import org.robockets.robot.pidsources.EncoderPIDSource; import org.robockets.robot.pidsources.GyroPIDSource; @@ -16,32 +16,25 @@ */ public class Drivetrain extends Subsystem { - private final EncoderPIDSource leftPodPIDSource; - private final EncoderPIDSource rightPodPIDSource; + private final EncoderPIDSource encoderPIDSource; private final GyroPIDSource gyroPIDSource; - public final PIDController leftPodPID; - public final PIDController rightPodPID; + public final PIDController encoderPID; public final PIDController gyroPID; - public Drivetrain() { + public final double ENCODER_KP = 0.18; - leftPodPIDSource = new EncoderPIDSource(RobotMap.leftEncoder); - leftPodPID = new PIDController(0.05, 0.00001, 0.00001, leftPodPIDSource, - new DrivePodPIDOutput(RobotMap.leftDrivepodSpeedController, true)); - leftPodPID.disable(); - leftPodPID.setOutputRange(-0.4, 0.4); - leftPodPID.setAbsoluteTolerance(2); + public Drivetrain() { - rightPodPIDSource = new EncoderPIDSource(RobotMap.rightEncoder); - rightPodPID = new PIDController(0.05, 0.00001, 0.00001, rightPodPIDSource, - new DrivePodPIDOutput(RobotMap.rightDrivepodSpeedController)); - rightPodPID.disable(); - rightPodPID.setOutputRange(-0.4, 0.4); - rightPodPID.setAbsoluteTolerance(2); + encoderPIDSource = new EncoderPIDSource(RobotMap.leftEncoder, RobotMap.rightEncoder); + encoderPID = new PIDController(0.076, 0.00001, 0.00001, encoderPIDSource, + new DummyPIDOutput()); // This might need to change + encoderPID.disable(); + encoderPID.setOutputRange(-0.6, 0.6); + encoderPID.setAbsoluteTolerance(2); gyroPIDSource = new GyroPIDSource(); - gyroPID = new PIDController(0.05, 0.0004, 0.15, gyroPIDSource, new GyroPIDOutput(RobotMap.robotDrive)); + gyroPID = new PIDController(0.057, 0.0005, 0.2, gyroPIDSource, new DummyPIDOutput()); gyroPID.disable(); gyroPID.setOutputRange(-0.75, 0.75); // Set turning speed range gyroPID.setAbsoluteTolerance(3); @@ -83,12 +76,8 @@ public void setGyroPID(double p, double i, double d) { gyroPID.setPID(p, i, d); } - public void setEncoderPID(RelativeDirection.XAxis side, double p, double i, double d) { - if (side == RelativeDirection.XAxis.LEFT) { - leftPodPID.setPID(p, i, d); - } else { - rightPodPID.setPID(p, i, d); - } + public void setEncoderPID(double p, double i, double d) { + encoderPID.setPID(p, i, d); } /** @@ -97,35 +86,11 @@ public void setEncoderPID(RelativeDirection.XAxis side, double p, double i, doub * @param distance Desired distance for both pods, in inches */ public void setDistance(double distance) { - leftPodPID.setSetpoint(distance); - rightPodPID.setSetpoint(distance); - } - - /** - * A basic method to set the setpoint of each of the drive pods for a given distance with PID - * - * @param leftDistance Desired distance for the left pod, in inches - * @param rightDistance Desired distance for the right pod, in inches - */ - public void setDistance(double leftDistance, double rightDistance) { - leftPodPID.setSetpoint(leftDistance); - rightPodPID.setSetpoint(rightDistance); - } - - /** - * Since the built in OnTarget for PID is terrible and broken, this is a manual one for the drive pods - * - * @return Returns if both the encoder PIDs are OnTarget, with a tolerance of ABSOLUTE_TOLERANCE - */ - public boolean encodersOnTarget() { - final double ABSOLUTE_TOLERANCE = 3.0; // TODO: Change this to something more reasonable - return Math.abs((leftPodPID.getSetpoint() - leftPodPIDSource.pidGet())) <= ABSOLUTE_TOLERANCE && - Math.abs((rightPodPID.getSetpoint() - rightPodPIDSource.pidGet())) <= ABSOLUTE_TOLERANCE; + encoderPID.setSetpoint(distance); } public void enableEncoderPID() { - leftPodPID.enable(); - rightPodPID.enable(); + encoderPID.enable(); } public void setGyroSetpoint(double angle) { @@ -141,12 +106,16 @@ public void disableGyroPID() { } public void disableEncoderPID() { - leftPodPID.disable(); - rightPodPID.disable(); + encoderPID.disable(); } public double getGyroPos() { return RobotMap.gyro.getAngle(); } + + public double getEncoderPos() { + //return (RobotMap.leftEncoder.get() + RobotMap.rightEncoder.get()) / 2.0; + return RobotMap.rightEncoder.getDistance(); + } } diff --git a/src/org/robockets/robot/drivetrain/StartEncoderPID.java b/src/org/robockets/robot/drivetrain/StartEncoderPID.java index 5b41fa9..79495b8 100644 --- a/src/org/robockets/robot/drivetrain/StartEncoderPID.java +++ b/src/org/robockets/robot/drivetrain/StartEncoderPID.java @@ -17,22 +17,20 @@ public StartEncoderPID() { } protected void initialize() { - drivetrain.setEncoderPID(RelativeDirection.XAxis.LEFT, SmartDashboard.getNumber("Left Encoder P", 0), + drivetrain.setEncoderPID(SmartDashboard.getNumber("Left Encoder P", 0), SmartDashboard.getNumber("Left Encoder I", 0), SmartDashboard.getNumber("Left Encoder D", 0)); - drivetrain.setEncoderPID(RelativeDirection.XAxis.RIGHT, SmartDashboard.getNumber("Right Encoder P", 0), - SmartDashboard.getNumber("Right Encoder I", 0), - SmartDashboard.getNumber("Right Encoder D", 0)); - drivetrain.setDistance(SmartDashboard.getNumber("Left Encoder Setpoint", 0), - SmartDashboard.getNumber("Right Encoder Setpoint", 0)); + drivetrain.setDistance(SmartDashboard.getNumber("Left Encoder Setpoint", 0)); drivetrain.enableEncoderPID(); } protected void execute() { + double angle = Robot.drivetrain.getGyroPos(); + Robot.drivetrain.driveArcade(Robot.drivetrain.encoderPID.get(), -angle*Robot.drivetrain.ENCODER_KP); } protected boolean isFinished() { - return false; + return Robot.drivetrain.encoderPID.onTarget(); } protected void end() { diff --git a/src/org/robockets/robot/drivetrain/StartGyroPID.java b/src/org/robockets/robot/drivetrain/StartGyroPID.java index ece4244..5e3c665 100644 --- a/src/org/robockets/robot/drivetrain/StartGyroPID.java +++ b/src/org/robockets/robot/drivetrain/StartGyroPID.java @@ -24,6 +24,7 @@ protected void initialize() { } protected void execute() { + drivetrain.driveArcade(0, drivetrain.gyroPID.get()); } protected boolean isFinished() { diff --git a/src/org/robockets/robot/drivetrain/TurnAbsolute.java b/src/org/robockets/robot/drivetrain/TurnAbsolute.java index d2937fa..6a227e7 100644 --- a/src/org/robockets/robot/drivetrain/TurnAbsolute.java +++ b/src/org/robockets/robot/drivetrain/TurnAbsolute.java @@ -12,6 +12,8 @@ public class TurnAbsolute extends Command { private double angle; + private final double TIMEOUT = 1.9; + public TurnAbsolute(double angle) { requires(Robot.drivetrain); this.angle = angle; @@ -20,16 +22,19 @@ public TurnAbsolute(double angle) { protected void initialize() { drivetrain.setGyroSetpoint(-angle); drivetrain.enableGyroPID(); + setTimeout(TIMEOUT); } protected void execute() { + drivetrain.driveArcade(0, -drivetrain.gyroPID.get()); } protected boolean isFinished() { - return drivetrain.gyroPID.onTarget(); + return drivetrain.gyroPID.onTarget() || isTimedOut(); } protected void end() { + System.out.println("Finished Turning"); drivetrain.disableGyroPID(); } diff --git a/src/org/robockets/robot/elevator/Elevator.java b/src/org/robockets/robot/elevator/Elevator.java index 4896a67..fb0fe2a 100644 --- a/src/org/robockets/robot/elevator/Elevator.java +++ b/src/org/robockets/robot/elevator/Elevator.java @@ -15,6 +15,10 @@ public class Elevator extends Subsystem { private final double TICKS_PER_INCH = 1.1428571428571428571428571428571; + private int elevatorTicks = 0; + + private RelativeDirection.ZAxis lastDirection = RelativeDirection.ZAxis.UP; + public Elevator() { RobotMap.elevatorMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, 0, 0); } @@ -29,6 +33,15 @@ public void initDefaultCommand() { } public void setElevatorSpeed(double elSpeed) { + + if (elSpeed < 0.1 && elSpeed > 0.1) { + lastDirection = RelativeDirection.ZAxis.DOWN; + } else if (elSpeed > 0) { + lastDirection = RelativeDirection.ZAxis.UP; + } else if (elSpeed < 0) { + lastDirection = RelativeDirection.ZAxis.DOWN; + } + RobotMap.elevatorMotor.set(elSpeed); //RobotMap.climberMotorRight.set(elSpeed); } @@ -61,7 +74,23 @@ public boolean isInPosition(ElevatorPosition position, RelativeDirection.ZAxis d } public double getRawEncoderPos() { - return RobotMap.counter.get(); + return elevatorTicks; + } + + public RelativeDirection.ZAxis getLastDirection() { + return lastDirection; + } + + public void countUp() { + elevatorTicks++; + } + + public void countDown() { + elevatorTicks--; + } + + public void resetCounter() { + elevatorTicks = 0; } public double getEncoderPos() { diff --git a/src/org/robockets/robot/elevator/ElevatorCounter.java b/src/org/robockets/robot/elevator/ElevatorCounter.java new file mode 100644 index 0000000..8c74dcc --- /dev/null +++ b/src/org/robockets/robot/elevator/ElevatorCounter.java @@ -0,0 +1,46 @@ +package org.robockets.robot.elevator; + +import edu.wpi.first.wpilibj.command.Command; +import org.robockets.commons.RelativeDirection; +import org.robockets.robot.Robot; +import org.robockets.robot.RobotMap; + +/** + * @author Jake Backer + */ +public class ElevatorCounter extends Command { + + private boolean wasActive = false; + + public ElevatorCounter() { + setRunWhenDisabled(true); + } + + protected void initialize() { + } + + protected void execute() { + boolean currentState = RobotMap.elevatorEncoder.get(); + + if (wasActive != currentState) { + if (Robot.elevator.getLastDirection() == RelativeDirection.ZAxis.UP) { + Robot.elevator.countUp(); + } else { + Robot.elevator.countDown(); + } + wasActive = currentState; + } + } + + protected boolean isFinished() { + return false; + } + + protected void end() { + + } + + protected void interrupted() { + end(); + } +} diff --git a/src/org/robockets/robot/elevator/ElevatorFloor.java b/src/org/robockets/robot/elevator/ElevatorFloor.java index 5827484..ebeb482 100644 --- a/src/org/robockets/robot/elevator/ElevatorFloor.java +++ b/src/org/robockets/robot/elevator/ElevatorFloor.java @@ -17,10 +17,10 @@ public void initDefaultCommand() { public void moveElevatorFloor(double speed) { - if (canMove()) { + //if (canMove()) { RobotMap.elevatorFloorMotor.set(speed); RobotMap.elevatorFloorMotor2.set(speed); - } + //} } public boolean canMove() { diff --git a/src/org/robockets/robot/elevator/ElevatorFloorListener.java b/src/org/robockets/robot/elevator/ElevatorFloorListener.java index 232f696..ecde3c8 100644 --- a/src/org/robockets/robot/elevator/ElevatorFloorListener.java +++ b/src/org/robockets/robot/elevator/ElevatorFloorListener.java @@ -17,7 +17,7 @@ protected void initialize() { } protected void execute() { - double speed = -OI.operatorJoystick.getRawAxis(1); + double speed = OI.operatorJoystick.getRawAxis(1); Robot.elevatorFloor.moveElevatorFloor(speed); } diff --git a/src/org/robockets/robot/elevator/ResetCounter.java b/src/org/robockets/robot/elevator/ResetCounter.java index ca0c2ce..12f5655 100644 --- a/src/org/robockets/robot/elevator/ResetCounter.java +++ b/src/org/robockets/robot/elevator/ResetCounter.java @@ -1,6 +1,7 @@ package org.robockets.robot.elevator; import edu.wpi.first.wpilibj.command.Command; +import org.robockets.robot.Robot; import org.robockets.robot.RobotMap; /** @@ -13,14 +14,15 @@ public ResetCounter() { } protected void initialize() { - RobotMap.counter.reset(); + setTimeout(1); + Robot.elevator.resetCounter(); } protected void execute() { } protected boolean isFinished() { - return true; + return isTimedOut(); } protected void end() { diff --git a/src/org/robockets/robot/elevator/TimedElevator.java b/src/org/robockets/robot/elevator/TimedElevator.java new file mode 100644 index 0000000..906aed8 --- /dev/null +++ b/src/org/robockets/robot/elevator/TimedElevator.java @@ -0,0 +1,39 @@ +package org.robockets.robot.elevator; + +import edu.wpi.first.wpilibj.command.Command; +import org.robockets.robot.Robot; + +/** + * @author Jake Backer + */ +public class TimedElevator extends Command { + + private double time; + private double speed; + + public TimedElevator(double time, double speed) { + this.time = time; + this.speed = speed; + } + + protected void initialize() { + setTimeout(time); + } + + protected void execute() { + System.out.println("ELEVATING"); + Robot.elevator.setElevatorSpeed(speed); + } + + protected boolean isFinished() { + return isTimedOut(); + } + + protected void end() { + + } + + protected void interrupted() { + end(); + } +} diff --git a/src/org/robockets/robot/pidsources/DummyPIDOutput.java b/src/org/robockets/robot/pidsources/DummyPIDOutput.java new file mode 100644 index 0000000..4b9015b --- /dev/null +++ b/src/org/robockets/robot/pidsources/DummyPIDOutput.java @@ -0,0 +1,10 @@ +package org.robockets.robot.pidsources; + +import edu.wpi.first.wpilibj.PIDOutput; + +public class DummyPIDOutput implements PIDOutput{ + @Override + public void pidWrite(double v) { + + } +} diff --git a/src/org/robockets/robot/pidsources/EncoderPIDSource.java b/src/org/robockets/robot/pidsources/EncoderPIDSource.java index 455ad13..abfa039 100644 --- a/src/org/robockets/robot/pidsources/EncoderPIDSource.java +++ b/src/org/robockets/robot/pidsources/EncoderPIDSource.java @@ -5,16 +5,17 @@ import edu.wpi.first.wpilibj.PIDSourceType; public class EncoderPIDSource implements PIDSource { - private Encoder encoder; + private Encoder encoder1; + private Encoder encoder2; + + private final double FACTOR = 1.0 / 39.92; /** * Encoder PID Source - * - * @param encoder The encoder that you wish to read values from. - * @param factor A multiplier to manipulate the encoder output. */ - public EncoderPIDSource(Encoder encoder) { - this.encoder = encoder; + public EncoderPIDSource(Encoder encoder1, Encoder encoder2) { + this.encoder1 = encoder1; + this.encoder2 = encoder2; } @Override @@ -28,6 +29,7 @@ public PIDSourceType getPIDSourceType() { @Override public double pidGet() { - return this.encoder.getDistance(); + //return (encoder1.getDistance() + encoder2.getDistance())/2.0; + return encoder2.getDistance(); } }