From 1e0f4ccfe97ba84173164c9850aa824d9e876433 Mon Sep 17 00:00:00 2001 From: Acelogic Date: Fri, 31 Mar 2017 11:13:39 -0400 Subject: [PATCH] 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(); }