From fa8773094e58a387afa1614b8fd491602166dd2b Mon Sep 17 00:00:00 2001 From: Miguel C Date: Thu, 30 Mar 2017 15:54:17 -0400 Subject: [PATCH 1/3] Changed vals back to normal --- src/org/usfirst/frc/team1635/robot/OI.java | 7 ++- src/org/usfirst/frc/team1635/robot/Robot.java | 1 + .../robot/commands/BottomOutElevator.java | 46 +++++++++++++++ .../team1635/robot/commands/ControlDrive.java | 2 +- .../robot/commands/GusGearIntake.java | 56 ------------------- .../robot/subsystems/ElevatorSubsystem.java | 21 +++++-- 6 files changed, 67 insertions(+), 66 deletions(-) create mode 100644 src/org/usfirst/frc/team1635/robot/commands/BottomOutElevator.java delete mode 100644 src/org/usfirst/frc/team1635/robot/commands/GusGearIntake.java diff --git a/src/org/usfirst/frc/team1635/robot/OI.java b/src/org/usfirst/frc/team1635/robot/OI.java index f357bcd..49a4b2e 100644 --- a/src/org/usfirst/frc/team1635/robot/OI.java +++ b/src/org/usfirst/frc/team1635/robot/OI.java @@ -1,6 +1,7 @@ package org.usfirst.frc.team1635.robot; -import org.usfirst.frc.team1635.robot.commands.GusGearIntake; +import org.usfirst.frc.team1635.robot.commands.BottomOutElevator; + import org.usfirst.frc.team1635.robot.commands.ShootBalls; import org.usfirst.frc.team1635.robot.commands.WiggleForward; @@ -96,8 +97,8 @@ public OI() { dPadUp.whenPressed(new ShootBalls()); dPadUp2.whenPressed(new ShootBalls()); - dPadDown.whenPressed(new GusGearIntake()); - dPadDown2.whenPressed(new GusGearIntake()); + dPadDown.whenPressed(new BottomOutElevator()); + dPadDown2.whenPressed(new BottomOutElevator()); // dPadLeft.whenPressed(new PopGearWithFlapsDown()); } diff --git a/src/org/usfirst/frc/team1635/robot/Robot.java b/src/org/usfirst/frc/team1635/robot/Robot.java index 24979b4..dfad11d 100644 --- a/src/org/usfirst/frc/team1635/robot/Robot.java +++ b/src/org/usfirst/frc/team1635/robot/Robot.java @@ -191,6 +191,7 @@ public void teleopPeriodic() { Timer.delay(0.2); forwardCameraOn = !(forwardCameraOn); } + Robot.oi.masterToSecondary(Robot.oi.StartController(), Robot.oi.StartController2()); SmartDashboard.putBoolean("forwardCameraOn", forwardCameraOn); // if (Robot.oi.gameController.getXButton()) { // Timer.delay(0.2); diff --git a/src/org/usfirst/frc/team1635/robot/commands/BottomOutElevator.java b/src/org/usfirst/frc/team1635/robot/commands/BottomOutElevator.java new file mode 100644 index 0000000..019dafa --- /dev/null +++ b/src/org/usfirst/frc/team1635/robot/commands/BottomOutElevator.java @@ -0,0 +1,46 @@ +package org.usfirst.frc.team1635.robot.commands; + +import org.usfirst.frc.team1635.robot.Robot; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class BottomOutElevator extends Command { + boolean isBottomedOut; + + public BottomOutElevator() { + requires(Robot.elevatorSystem); + } + + // Called just before this Command runs the first time + protected void initialize() { + + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + isBottomedOut = Robot.elevatorSystem.IsElevatorBottomedOut(); + Robot.elevatorSystem.setElevatorSpeed(-0.5); + + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return isBottomedOut; + } + + // Called once after isFinished returns true + protected void end() { + Robot.elevatorSystem.stopElevator(); + + } + + // 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/usfirst/frc/team1635/robot/commands/ControlDrive.java b/src/org/usfirst/frc/team1635/robot/commands/ControlDrive.java index f662690..16081be 100644 --- a/src/org/usfirst/frc/team1635/robot/commands/ControlDrive.java +++ b/src/org/usfirst/frc/team1635/robot/commands/ControlDrive.java @@ -36,7 +36,7 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run protected void execute() { Robot.chassisSystem.drive(); - Robot.oi.masterToSecondary(Robot.oi.StartController(), Robot.oi.StartController2()); + Robot.chassisSystem.log(); } diff --git a/src/org/usfirst/frc/team1635/robot/commands/GusGearIntake.java b/src/org/usfirst/frc/team1635/robot/commands/GusGearIntake.java deleted file mode 100644 index 45bfb61..0000000 --- a/src/org/usfirst/frc/team1635/robot/commands/GusGearIntake.java +++ /dev/null @@ -1,56 +0,0 @@ -package org.usfirst.frc.team1635.robot.commands; - -import org.usfirst.frc.team1635.robot.Robot; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.command.Command; - -/** - * - */ -public class GusGearIntake extends Command { - - public GusGearIntake() { - requires(Robot.elevatorSystem); - requires(Robot.pneumaticsSystem); - } - - // Called just before this Command runs the first time - protected void initialize() { - Robot.elevatorSystem.setRollerState(false); - Robot.elevatorSystem.moveFlapsDown(); - Timer.delay(.3); - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - if (Robot.elevatorSystem.getBottomLimit()) { - Robot.elevatorSystem.stopElevator(); - } else { - Robot.elevatorSystem.setElevatorSpeed(-0.5); - } - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - boolean hitBottom = Robot.elevatorSystem.getBottomLimit(); - System.out.println("Debug: Checking for Bottom"); - if (hitBottom) { - System.out.println("Debug:Hit Bottom"); - } - return hitBottom; - } - - // Called once after isFinished returns true - protected void end() { - Robot.elevatorSystem.stopElevator(); - Robot.elevatorSystem.setFlapsDown(false); - - } - - // 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/usfirst/frc/team1635/robot/subsystems/ElevatorSubsystem.java b/src/org/usfirst/frc/team1635/robot/subsystems/ElevatorSubsystem.java index 7f960af..e4a2b4d 100644 --- a/src/org/usfirst/frc/team1635/robot/subsystems/ElevatorSubsystem.java +++ b/src/org/usfirst/frc/team1635/robot/subsystems/ElevatorSubsystem.java @@ -68,10 +68,10 @@ public void initDefaultCommand() { public void controlElevator() { if ((Robot.oi.StartController().getTriggerAxis(Hand.kLeft) > .3)) { - setElevatorSpeed(-0.7); + setElevatorSpeed(-0.675); } else if (Robot.oi.StartController().getTriggerAxis(Hand.kRight) > .3) { - setElevatorSpeed(0.7); + setElevatorSpeed(0.675); // if(isElevatorAtSweetSpot()){ // stopElevator(); @@ -99,7 +99,7 @@ public void controlElevator() { } public void elevatorRollerControl() { - if (Robot.oi.StartController().getXButton()) { + if (Robot.oi.globalX) { elevatorRoller.set(1); } else { @@ -108,7 +108,7 @@ public void elevatorRollerControl() { } public void controlFlaps() { - if (Robot.oi.StartController().getStartButton()) { + if (Robot.oi.globalStartButton) { moveFlapsDown(); Timer.delay(0.1); @@ -130,10 +130,10 @@ public void log() { public void setRollerState(boolean status) { if (status) { elevatorRoller.set(1); - + } else elevatorRoller.set(0); - + } public void setFlapsDown(boolean flapsDown) { @@ -175,6 +175,15 @@ public boolean isElevatorAtDangerSpot() { } } + + public boolean IsElevatorBottomedOut(){ + if(Math.abs(getPotentiometerValue() - 635.0) <=3 ){ + return true; + } + else{ + return false; + } + } public double getPotentiometerValue() { double potVal = analogPot.get(); From e95119607dcbebd317c20afb7fe8e57dc0a20a53 Mon Sep 17 00:00:00 2001 From: Miguel C Date: Thu, 30 Mar 2017 17:50:49 -0400 Subject: [PATCH 2/3] Latest Code --- .../autonomous/AutonomousVisionLeft.java | 5 +- src/org/usfirst/frc/team1635/robot/Robot.java | 3 +- .../{commands => control}/ControlDrive.java | 2 +- .../ControlElevator.java | 2 +- .../ControlPneumatics.java | 2 +- .../{commands => control}/ControlWinch.java | 2 +- .../robot/subsystems/ChassisSubsystem.java | 3 +- .../robot/subsystems/ElevatorSubsystem.java | 42 ++++++--------- .../robot/subsystems/PneumaticsSubsystem.java | 54 ++++++------------- .../robot/subsystems/WinchClimbSubsystem.java | 27 ++-------- 10 files changed, 46 insertions(+), 96 deletions(-) rename src/org/usfirst/frc/team1635/robot/{commands => control}/ControlDrive.java (96%) rename src/org/usfirst/frc/team1635/robot/{commands => control}/ControlElevator.java (95%) rename src/org/usfirst/frc/team1635/robot/{commands => control}/ControlPneumatics.java (95%) rename src/org/usfirst/frc/team1635/robot/{commands => control}/ControlWinch.java (94%) diff --git a/src/org/usfirst/frc/team1635/autonomous/AutonomousVisionLeft.java b/src/org/usfirst/frc/team1635/autonomous/AutonomousVisionLeft.java index c602595..0f3573b 100644 --- a/src/org/usfirst/frc/team1635/autonomous/AutonomousVisionLeft.java +++ b/src/org/usfirst/frc/team1635/autonomous/AutonomousVisionLeft.java @@ -19,9 +19,8 @@ public AutonomousVisionLeft() { addSequential(new TurnToSetPointLi(RobotMap.autoLeftTurnRight, true)); addSequential(new DriveWithVision()); // next method takes double executionTime, double speed - addSequential(new TimeoutDriveWithCorrectionSlow( - RobotMap.autoVisionStraightTime - , RobotMap.autoVisionStraightSpeed)); + addSequential( + new TimeoutDriveWithCorrectionSlow(RobotMap.autoVisionStraightTime, RobotMap.autoVisionStraightSpeed)); addSequential(new WiggleForward()); } } diff --git a/src/org/usfirst/frc/team1635/robot/Robot.java b/src/org/usfirst/frc/team1635/robot/Robot.java index dfad11d..6947982 100644 --- a/src/org/usfirst/frc/team1635/robot/Robot.java +++ b/src/org/usfirst/frc/team1635/robot/Robot.java @@ -187,11 +187,12 @@ public void disabledInit() { */ public void teleopPeriodic() { Scheduler.getInstance().run(); + Robot.oi.masterToSecondary(Robot.oi.StartController(), Robot.oi.StartController2()); if (Robot.oi.globalB) { Timer.delay(0.2); forwardCameraOn = !(forwardCameraOn); } - Robot.oi.masterToSecondary(Robot.oi.StartController(), Robot.oi.StartController2()); + SmartDashboard.putBoolean("forwardCameraOn", forwardCameraOn); // if (Robot.oi.gameController.getXButton()) { // Timer.delay(0.2); diff --git a/src/org/usfirst/frc/team1635/robot/commands/ControlDrive.java b/src/org/usfirst/frc/team1635/robot/control/ControlDrive.java similarity index 96% rename from src/org/usfirst/frc/team1635/robot/commands/ControlDrive.java rename to src/org/usfirst/frc/team1635/robot/control/ControlDrive.java index 16081be..b4a622d 100644 --- a/src/org/usfirst/frc/team1635/robot/commands/ControlDrive.java +++ b/src/org/usfirst/frc/team1635/robot/control/ControlDrive.java @@ -1,4 +1,4 @@ -package org.usfirst.frc.team1635.robot.commands; +package org.usfirst.frc.team1635.robot.control; //Local Package Imports import org.usfirst.frc.team1635.robot.Robot; diff --git a/src/org/usfirst/frc/team1635/robot/commands/ControlElevator.java b/src/org/usfirst/frc/team1635/robot/control/ControlElevator.java similarity index 95% rename from src/org/usfirst/frc/team1635/robot/commands/ControlElevator.java rename to src/org/usfirst/frc/team1635/robot/control/ControlElevator.java index 09086f2..dea9685 100644 --- a/src/org/usfirst/frc/team1635/robot/commands/ControlElevator.java +++ b/src/org/usfirst/frc/team1635/robot/control/ControlElevator.java @@ -1,4 +1,4 @@ -package org.usfirst.frc.team1635.robot.commands; +package org.usfirst.frc.team1635.robot.control; import org.usfirst.frc.team1635.robot.Robot; diff --git a/src/org/usfirst/frc/team1635/robot/commands/ControlPneumatics.java b/src/org/usfirst/frc/team1635/robot/control/ControlPneumatics.java similarity index 95% rename from src/org/usfirst/frc/team1635/robot/commands/ControlPneumatics.java rename to src/org/usfirst/frc/team1635/robot/control/ControlPneumatics.java index 436e449..1600851 100644 --- a/src/org/usfirst/frc/team1635/robot/commands/ControlPneumatics.java +++ b/src/org/usfirst/frc/team1635/robot/control/ControlPneumatics.java @@ -1,4 +1,4 @@ -package org.usfirst.frc.team1635.robot.commands; +package org.usfirst.frc.team1635.robot.control; import org.usfirst.frc.team1635.robot.Robot; diff --git a/src/org/usfirst/frc/team1635/robot/commands/ControlWinch.java b/src/org/usfirst/frc/team1635/robot/control/ControlWinch.java similarity index 94% rename from src/org/usfirst/frc/team1635/robot/commands/ControlWinch.java rename to src/org/usfirst/frc/team1635/robot/control/ControlWinch.java index 77e305c..bc37a81 100644 --- a/src/org/usfirst/frc/team1635/robot/commands/ControlWinch.java +++ b/src/org/usfirst/frc/team1635/robot/control/ControlWinch.java @@ -1,4 +1,4 @@ -package org.usfirst.frc.team1635.robot.commands; +package org.usfirst.frc.team1635.robot.control; import org.usfirst.frc.team1635.robot.Robot; diff --git a/src/org/usfirst/frc/team1635/robot/subsystems/ChassisSubsystem.java b/src/org/usfirst/frc/team1635/robot/subsystems/ChassisSubsystem.java index d9c757b..fd4cb24 100644 --- a/src/org/usfirst/frc/team1635/robot/subsystems/ChassisSubsystem.java +++ b/src/org/usfirst/frc/team1635/robot/subsystems/ChassisSubsystem.java @@ -3,8 +3,7 @@ // Local Package Imports import org.usfirst.frc.team1635.robot.Robot; import org.usfirst.frc.team1635.robot.RobotMap; -import org.usfirst.frc.team1635.robot.commands.ControlDrive; -//------------------------------------------------------------ +import org.usfirst.frc.team1635.robot.control.ControlDrive; // CTRE Imports import com.ctre.CANTalon; diff --git a/src/org/usfirst/frc/team1635/robot/subsystems/ElevatorSubsystem.java b/src/org/usfirst/frc/team1635/robot/subsystems/ElevatorSubsystem.java index e4a2b4d..f214993 100644 --- a/src/org/usfirst/frc/team1635/robot/subsystems/ElevatorSubsystem.java +++ b/src/org/usfirst/frc/team1635/robot/subsystems/ElevatorSubsystem.java @@ -4,6 +4,7 @@ import org.usfirst.frc.team1635.robot.Robot; import org.usfirst.frc.team1635.robot.RobotMap; import org.usfirst.frc.team1635.robot.commands.*; +import org.usfirst.frc.team1635.robot.control.ControlElevator; import org.usfirst.frc.team1635.util.XboxControllerButton; import com.ctre.CANTalon; @@ -71,20 +72,16 @@ public void controlElevator() { setElevatorSpeed(-0.675); } else if (Robot.oi.StartController().getTriggerAxis(Hand.kRight) > .3) { - setElevatorSpeed(0.675); - - // if(isElevatorAtSweetSpot()){ - // stopElevator(); - // Timer.delay(0.3); - // System.out.println("Debug. SweetSpot Triggered"); - // } - // if(isElevatorAtDangerSpot() && !getFlapState()){ - // stopElevator(); - // setFlapsDown(true); - // Timer.delay(0.3); - // System.out.println("Debug. DangerZone Triggered"); - // } - + if (isElevatorAtDangerSpot()) { + stopElevator(); + Timer.delay(0.2); + setElevatorSpeed(-0.15); + Timer.delay(1.25); + } + + else { + setElevatorSpeed(0.675); + } } else { @@ -140,10 +137,6 @@ public void setFlapsDown(boolean flapsDown) { this.flapsDown = flapsDown; } - public void moveFlapsForGears() { - - } - public void moveFlapsUp() { flapsSolenoid.set(false); } @@ -175,13 +168,12 @@ public boolean isElevatorAtDangerSpot() { } } - - public boolean IsElevatorBottomedOut(){ - if(Math.abs(getPotentiometerValue() - 635.0) <=3 ){ - return true; - } - else{ - return false; + + public boolean IsElevatorBottomedOut() { + if (Math.abs(getPotentiometerValue() - 635.0) <= 3) { + return true; + } else { + return false; } } diff --git a/src/org/usfirst/frc/team1635/robot/subsystems/PneumaticsSubsystem.java b/src/org/usfirst/frc/team1635/robot/subsystems/PneumaticsSubsystem.java index 3ec5a6e..0041767 100644 --- a/src/org/usfirst/frc/team1635/robot/subsystems/PneumaticsSubsystem.java +++ b/src/org/usfirst/frc/team1635/robot/subsystems/PneumaticsSubsystem.java @@ -2,8 +2,7 @@ import org.usfirst.frc.team1635.robot.Robot; import org.usfirst.frc.team1635.robot.RobotMap; -import org.usfirst.frc.team1635.robot.commands.ControlPneumatics; - +import org.usfirst.frc.team1635.robot.control.ControlPneumatics; import edu.wpi.first.wpilibj.Compressor; import edu.wpi.first.wpilibj.DoubleSolenoid; @@ -19,16 +18,15 @@ */ public class PneumaticsSubsystem extends Subsystem { Compressor compressor; - Solenoid gearSolenoid; + Solenoid flapsSolenoid; Solenoid gearShifter; - public PneumaticsSubsystem() { super(); compressor = new Compressor(RobotMap.compressorPort); - gearSolenoid = new Solenoid(RobotMap.gearSolenoidPort); + flapsSolenoid = new Solenoid(RobotMap.gearSolenoidPort); gearShifter = new Solenoid(RobotMap.gearShifterPort); - + } // Whatever command you set as default will run when the enable button is @@ -44,50 +42,28 @@ public void initDefaultCommand() { // ------------------------------------------------------------ public void shiftDriveGears() { if (Robot.oi.StartController().getBumper(Hand.kRight)) { - gearShifter.set(true); - Timer.delay(0.05); - } else if(Robot.oi.StartController().getBumper(Hand.kLeft)){ - gearShifter.set(false); - Timer.delay(0.05); - } + gearShifter.set(true); + Timer.delay(0.05); + } else if (Robot.oi.StartController().getBumper(Hand.kLeft)) { + gearShifter.set(false); + Timer.delay(0.05); } - - - public void controlGearPiston() { -// if (Robot.oi.StartController().getBButton()) { -// -// } } - - - // Functions Dedicated for Automous Mode or General Purpose Commands + // Functions Dedicated for Autonomous Mode or General Purpose Commands // ------------------------------------------------------------ - public void log(){ - SmartDashboard.putBoolean("FlapState", gearSolenoid.get()); + public void log() { + SmartDashboard.putBoolean("FlapState", flapsSolenoid.get()); } - public void popGear(){ - extendGearPiston(); - Timer.delay(0.5); - retractGearPiston(); - - } - public void setHighGear(){ + public void setHighGear() { gearShifter.set(false); Timer.delay(0.1); } - - public void setLowGear(){ + + public void setLowGear() { gearShifter.set(true); Timer.delay(0.1); } - public void extendGearPiston() { - gearSolenoid.set(true); - } - - public void retractGearPiston() { - gearSolenoid.set(false); - } } diff --git a/src/org/usfirst/frc/team1635/robot/subsystems/WinchClimbSubsystem.java b/src/org/usfirst/frc/team1635/robot/subsystems/WinchClimbSubsystem.java index 4f3ede4..d0676d7 100644 --- a/src/org/usfirst/frc/team1635/robot/subsystems/WinchClimbSubsystem.java +++ b/src/org/usfirst/frc/team1635/robot/subsystems/WinchClimbSubsystem.java @@ -8,11 +8,11 @@ //Local Package Imports import org.usfirst.frc.team1635.robot.RobotMap; +import org.usfirst.frc.team1635.robot.control.ControlWinch; import org.omg.CORBA.ExceptionList; import org.omg.IOP.ExceptionDetailMessage; import org.usfirst.frc.team1635.robot.OI; import org.usfirst.frc.team1635.robot.Robot; -import org.usfirst.frc.team1635.robot.commands.ControlWinch; //.---. ,--. .-----. .------. ///_ | / .' / -. \| ___| @@ -50,38 +50,21 @@ public void initDefaultCommand() { // Functions Utilizing the Xbox Controller's Buttons or Axes // ------------------------------------------------------------ public void operateWinch() { - - if ( Robot.oi.globalBackButton) { + + if (Robot.oi.globalBackButton) { rollerTalonSR.set(-1); rollerTalonSRx2.set(-1); - } - else if(!Robot.oi.globalBackButton){ + } else if (!Robot.oi.globalBackButton) { stopWinch(); } } // Functions Dedicated for Automous Mode or General Purpose Commands - // ------------------------------------------------------------ - public void setWinchParams(double finalInput) { - System.out.println("RollerWithParamsActivated"); - rollerTalonSR.set(finalInput); - // rollerTalonSRx2.set(finalInput); - } + // ----------------------------------------------------------- public void stopWinch() { rollerTalonSR.set(0); rollerTalonSRx2.set(0); } - public double getTalonSRLastValue() { - double getTalonSRValue = rollerTalonSR.get(); - return getTalonSRValue; - - } - - public double getTalonSRx2LastValue() { - double getTalonSRx2Value = rollerTalonSRx2.get(); - return getTalonSRx2Value; - } - } From 1e0f4ccfe97ba84173164c9850aa824d9e876433 Mon Sep 17 00:00:00 2001 From: Acelogic Date: Fri, 31 Mar 2017 11:13:39 -0400 Subject: [PATCH 3/3] CAN ID 4 Talon giving problems on drive train, replaced with CAN ID 2 Talon --- src/org/usfirst/frc/team1635/robot/OI.java | 11 +-- .../usfirst/frc/team1635/robot/RobotMap.java | 4 +- .../team1635/robot/commands/ShootBalls.java | 76 ------------------- .../team1635/robot/control/ControlDrive.java | 73 +++++++++--------- .../robot/control/ControlElevator.java | 1 - .../robot/control/ControlPneumatics.java | 1 - .../robot/subsystems/ElevatorSubsystem.java | 16 +--- .../robot/subsystems/WinchClimbSubsystem.java | 4 +- 8 files changed, 45 insertions(+), 141 deletions(-) delete mode 100644 src/org/usfirst/frc/team1635/robot/commands/ShootBalls.java diff --git a/src/org/usfirst/frc/team1635/robot/OI.java b/src/org/usfirst/frc/team1635/robot/OI.java index 49a4b2e..8f00d30 100644 --- a/src/org/usfirst/frc/team1635/robot/OI.java +++ b/src/org/usfirst/frc/team1635/robot/OI.java @@ -2,8 +2,6 @@ import org.usfirst.frc.team1635.robot.commands.BottomOutElevator; - -import org.usfirst.frc.team1635.robot.commands.ShootBalls; import org.usfirst.frc.team1635.robot.commands.WiggleForward; //Local Package Imports import org.usfirst.frc.team1635.util.XboxControllerButton; @@ -52,8 +50,8 @@ public class OI { DPadButton dPadLeft = new DPadButton(gameController, DPadButton.Direction.Left); DPadButton dPadRight = new DPadButton(gameController, DPadButton.Direction.Right); -// ----------------------------------------------------------------------------------- -// For DualControllers + // ----------------------------------------------------------------------------------- + // For DualControllers public boolean globalA; public boolean globalB; public boolean globalX; @@ -87,16 +85,13 @@ public void masterToSecondary(XboxController master, XboxController secondary) { this.globalStartButton = master.getStartButton() || secondary.getStartButton(); } -// --------------------------------------------------------------------------------------- + // --------------------------------------------------------------------------------------- public OI() { aButton.whenPressed(new WiggleForward()); aButton2.whenPressed(new WiggleForward()); // Don't use B - we switch camera with it. - dPadUp.whenPressed(new ShootBalls()); - dPadUp2.whenPressed(new ShootBalls()); - dPadDown.whenPressed(new BottomOutElevator()); dPadDown2.whenPressed(new BottomOutElevator()); // dPadLeft.whenPressed(new PopGearWithFlapsDown()); diff --git a/src/org/usfirst/frc/team1635/robot/RobotMap.java b/src/org/usfirst/frc/team1635/robot/RobotMap.java index a566b52..0a8292e 100644 --- a/src/org/usfirst/frc/team1635/robot/RobotMap.java +++ b/src/org/usfirst/frc/team1635/robot/RobotMap.java @@ -19,9 +19,9 @@ public class RobotMap { // ------------------------------------------------------------ // CAN IDs/Ports public static int elevatorMotorCANPort = 3; - public static int elevatorRollerMotorCANPort = 2; + public static int elevatorRollerMotorCANPort; // Drive - public static int frontLeftMotorCANPort = 4; + public static int frontLeftMotorCANPort = 2; // 4 Died Replacing with 2 public static int frontRightMotorCANPort = 6; public static int backLeftMotorCANPort = 1; public static int backRightMotorCANPort = 5; diff --git a/src/org/usfirst/frc/team1635/robot/commands/ShootBalls.java b/src/org/usfirst/frc/team1635/robot/commands/ShootBalls.java deleted file mode 100644 index 0f693c1..0000000 --- a/src/org/usfirst/frc/team1635/robot/commands/ShootBalls.java +++ /dev/null @@ -1,76 +0,0 @@ -package org.usfirst.frc.team1635.robot.commands; - -import edu.wpi.first.wpilibj.command.Command; -import org.usfirst.frc.team1635.robot.Robot; -/** - * - */ -public class ShootBalls extends Command { - - private int state = 1; - // 1 = We are below the danger point - // 2 = We are at the danger point (lower flaps) - // 3 = We are between the danger point and the sweet spot - // 4 = we are at the sweet spot. stop this command and get out of here. - - public ShootBalls() { - requires(Robot.elevatorSystem); - requires(Robot.pneumaticsSystem ); - } - - // Called just before this Command runs the first time - protected void initialize() { - if (Robot.elevatorSystem.getPotentiometerValue() > 520) { - state = 1; - } else { - state = 4; - } - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - Robot.elevatorSystem.setRollerState(true); - switch (state) { - case 1: - if (Robot.elevatorSystem.isElevatorAtDangerSpot()) { - Robot.elevatorSystem.stopElevator(); - state = 2; - } else { - Robot.elevatorSystem.setElevatorSpeed(.5); - } - break; - case 2: - Robot.elevatorSystem.moveFlapsDown(); - state = 3; - break; - case 3: - if (Robot.elevatorSystem.isElevatorAtSweetSpot()) { - Robot.elevatorSystem.stopElevator(); - state = 4; - } else { - Robot.elevatorSystem.setElevatorSpeed(.5); - } - break; - default: - state = 4; - } - - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return (state == 4); - } - - // Called once after isFinished returns true - protected void end() { - Robot.elevatorSystem.stopElevator(); - Robot.elevatorSystem.setFlapsDown(true); - } - - // 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/usfirst/frc/team1635/robot/control/ControlDrive.java b/src/org/usfirst/frc/team1635/robot/control/ControlDrive.java index b4a622d..97a1573 100644 --- a/src/org/usfirst/frc/team1635/robot/control/ControlDrive.java +++ b/src/org/usfirst/frc/team1635/robot/control/ControlDrive.java @@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj.command.Command; //------------------------------------------------------------ - //.---. ,--. .-----. .------. ///_ | / .' / -. \| ___| //| |. / -. '-' _' || '--. @@ -17,42 +16,42 @@ //`---' `----' `----'' `----'' /** -* -* @author Bogdan Bradu & Miguel Cruz ( @Acelogic_) -* -*/ + * + * @author Bogdan Bradu & Miguel Cruz ( @Acelogic_) + * + */ public class ControlDrive extends Command { - public ControlDrive() { - // Use requires() here to declare subsystem dependencies - requires(Robot.chassisSystem); - - } - - // Called just before this Command runs the first time - protected void initialize() { - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - Robot.chassisSystem.drive(); - - Robot.chassisSystem.log(); - - } - - // 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.chassisSystem.stop(); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - } + public ControlDrive() { + // Use requires() here to declare subsystem dependencies + requires(Robot.chassisSystem); + + } + + // Called just before this Command runs the first time + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + Robot.oi.masterToSecondary(Robot.oi.StartController(), Robot.oi.StartController2()); + Robot.chassisSystem.drive(); + Robot.chassisSystem.log(); + + } + + // 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.chassisSystem.stop(); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } } diff --git a/src/org/usfirst/frc/team1635/robot/control/ControlElevator.java b/src/org/usfirst/frc/team1635/robot/control/ControlElevator.java index dea9685..b4f673f 100644 --- a/src/org/usfirst/frc/team1635/robot/control/ControlElevator.java +++ b/src/org/usfirst/frc/team1635/robot/control/ControlElevator.java @@ -22,7 +22,6 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run protected void execute() { Robot.elevatorSystem.controlElevator(); - Robot.elevatorSystem.elevatorRollerControl(); Robot.elevatorSystem.log(); } diff --git a/src/org/usfirst/frc/team1635/robot/control/ControlPneumatics.java b/src/org/usfirst/frc/team1635/robot/control/ControlPneumatics.java index 1600851..32b3c9d 100644 --- a/src/org/usfirst/frc/team1635/robot/control/ControlPneumatics.java +++ b/src/org/usfirst/frc/team1635/robot/control/ControlPneumatics.java @@ -21,7 +21,6 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run protected void execute() { - Robot.pneumaticsSystem.shiftDriveGears(); Robot.elevatorSystem.controlFlaps(); Robot.pneumaticsSystem.log(); diff --git a/src/org/usfirst/frc/team1635/robot/subsystems/ElevatorSubsystem.java b/src/org/usfirst/frc/team1635/robot/subsystems/ElevatorSubsystem.java index f214993..9fbf7cf 100644 --- a/src/org/usfirst/frc/team1635/robot/subsystems/ElevatorSubsystem.java +++ b/src/org/usfirst/frc/team1635/robot/subsystems/ElevatorSubsystem.java @@ -45,7 +45,7 @@ public class ElevatorSubsystem extends Subsystem { public ElevatorSubsystem() { super(); elevatorActuator = new CANTalon(RobotMap.elevatorMotorCANPort); - elevatorRoller = new CANTalon(RobotMap.elevatorRollerMotorCANPort); + limitSwitchTop = new DigitalInput(RobotMap.topLimitSwitchDioPort); limitSwitchBottom = new DigitalInput(RobotMap.bottomLimitSwitchDioPort); analogPot = new AnalogPotentiometer(RobotMap.potentiometerAnalogPort, 3600.0 / 5); @@ -74,9 +74,6 @@ public void controlElevator() { } else if (Robot.oi.StartController().getTriggerAxis(Hand.kRight) > .3) { if (isElevatorAtDangerSpot()) { stopElevator(); - Timer.delay(0.2); - setElevatorSpeed(-0.15); - Timer.delay(1.25); } else { @@ -95,15 +92,6 @@ public void controlElevator() { } } - public void elevatorRollerControl() { - if (Robot.oi.globalX) { - elevatorRoller.set(1); - - } else { - elevatorRoller.set(0); - } - } - public void controlFlaps() { if (Robot.oi.globalStartButton) { moveFlapsDown(); @@ -161,7 +149,7 @@ public boolean isElevatorAtSweetSpot() { } public boolean isElevatorAtDangerSpot() { - if (Math.abs(getPotentiometerValue() - 520.0) <= 5) { + if (Math.abs(getPotentiometerValue() - 394.0) <= 5) { return true; } else { return false; diff --git a/src/org/usfirst/frc/team1635/robot/subsystems/WinchClimbSubsystem.java b/src/org/usfirst/frc/team1635/robot/subsystems/WinchClimbSubsystem.java index d0676d7..3a56802 100644 --- a/src/org/usfirst/frc/team1635/robot/subsystems/WinchClimbSubsystem.java +++ b/src/org/usfirst/frc/team1635/robot/subsystems/WinchClimbSubsystem.java @@ -52,8 +52,8 @@ public void initDefaultCommand() { public void operateWinch() { if (Robot.oi.globalBackButton) { - rollerTalonSR.set(-1); - rollerTalonSRx2.set(-1); + rollerTalonSR.set(1); + rollerTalonSRx2.set(1); } else if (!Robot.oi.globalBackButton) { stopWinch(); }