From: Logan Howard Date: Fri, 17 Apr 2015 03:00:51 +0000 (-0700) Subject: everything done except mapping buttons to actions X-Git-Url: http://challenge-bot.com/repos/?p=3501%2F3501-spark-go;a=commitdiff_plain;h=f24c549dc374aed0a8b543e1c64139ef0318b65b everything done except mapping buttons to actions --- diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java index 0b8fa07..6a24558 100644 --- a/src/org/usfirst/frc/team3501/robot/OI.java +++ b/src/org/usfirst/frc/team3501/robot/OI.java @@ -3,7 +3,7 @@ package org.usfirst.frc.team3501.robot; import org.usfirst.frc.team3501.robot.commands.*; public class OI { - public Joystick left, right; + private Joystick left, right; public OI() { left = new Joystick(RobotMap.LEFT_JOYSTICK_PORT); @@ -13,11 +13,15 @@ public class OI { right.whenReleased(1, new OpenClaw()); } - public double getForward() { + public double getForwardL() { + return left.getY(); + } + + public double getForwardR() { return right.getY(); } - public double getTwist() { + public double getTwistR() { return right.getTwist(); } } diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index b5ac5b7..23a43b7 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -13,9 +13,11 @@ import org.usfirst.frc.team3501.robot.subsystems.*; public class Robot extends IterativeRobot { - public static final Drivetrain drivetrain = new Drivetrain(); - public static final Arm arm = new Arm(); - public static final Claw claw = new Claw(); + public static Drivetrain drivetrain; + public static Arm arm; + public static Claw claw; + + public static Pneumatics pneumatics; public static OI oi; @@ -26,6 +28,12 @@ public class Robot extends IterativeRobot { public void robotInit() { oi = new OI(); + drivetrain = new Drivetrain(); + arm = new Arm(); + claw = new Claw(); + + pneumatics = new Pneumatics(); + chooseAuto(); } @@ -34,6 +42,8 @@ public class Robot extends IterativeRobot { } public void autonomousInit() { + pneumatics.start(); + autonomousCommand = (Command) autoChooser.getSelected(); autonomousCommand.start(); } @@ -43,6 +53,8 @@ public class Robot extends IterativeRobot { } public void teleopInit() { + pneumatics.start(); + autonomousCommand.cancel(); } @@ -54,6 +66,10 @@ public class Robot extends IterativeRobot { LiveWindow.run(); } + public void disabledInit() { + pneumatics.stop(); + } + private void chooseAuto() { autoChooser = new SendableChooser(); diff --git a/src/org/usfirst/frc/team3501/robot/RobotMap.java b/src/org/usfirst/frc/team3501/robot/RobotMap.java index bfb2bea..3ce51bf 100644 --- a/src/org/usfirst/frc/team3501/robot/RobotMap.java +++ b/src/org/usfirst/frc/team3501/robot/RobotMap.java @@ -3,27 +3,27 @@ package org.usfirst.frc.team3501.robot; import edu.wpi.first.wpilibj.DoubleSolenoid.Value; public class RobotMap { - // Driver Station + // driver station public static final int LEFT_JOYSTICK_PORT = 0, RIGHT_JOYSTICK_PORT = 1; public static final double MIN_DRIVE_JOYSTICK_INPUT = 0.1, MIN_ARM_JOYSTICK_INPUT = 0.1; - // Drivetrain + // drivetrain public static final int FRONT_LEFT_ADDRESS = 4, FRONT_RIGHT_ADDRESS = 5, REAR_LEFT_ADDRESS = 3, REAR_RIGHT_ADDRESS = 6; public static final double MAX_DRIVE_SPEED = 0.7; - // Claw + // claw public static final int CLAW_FORWARD_CHANNEL = 0, CLAW_REVERSE_CHANNEL = 1; public static final Value OPEN = Value.kForward, CLOSED = Value.kReverse; - // Arm + // arm public static final int LEFT_WINCH_ADDRESS = 2, RIGHT_WINCH_ADDRESS = 7; - // Auton + // auton public static final double OVER_STEP_TIME = 1.2, OVER_STEP_SPEED = 0.7, PAST_STEP_TIME = 1.5, PAST_STEP_SPEED = 0.5; } diff --git a/src/org/usfirst/frc/team3501/robot/commands/DriveWithJoysticks.java b/src/org/usfirst/frc/team3501/robot/commands/DriveWithJoysticks.java index 500bad9..cda6cdd 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/DriveWithJoysticks.java +++ b/src/org/usfirst/frc/team3501/robot/commands/DriveWithJoysticks.java @@ -8,7 +8,7 @@ public class DriveWithJoysticks extends CommandBase { } protected void execute() { - drivetrain.drive(oi.getForward(), oi.getTwist()); + drivetrain.drive(oi.getForwardR(), oi.getTwistR()); } protected boolean isFinished() { diff --git a/src/org/usfirst/frc/team3501/robot/commands/MoveArm.java b/src/org/usfirst/frc/team3501/robot/commands/MoveArm.java new file mode 100644 index 0000000..f0865af --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commands/MoveArm.java @@ -0,0 +1,17 @@ +package org.usfirst.frc.team3501.robot.commands; + +public class MoveArm extends CommandBase { + + public MoveArm() { + super("MoveArm"); + requires(arm); + } + + protected void execute() { + arm.setFromJoystick(oi.getForwardL()); + } + + protected boolean isFinished() { + return false; + } +} diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Arm.java b/src/org/usfirst/frc/team3501/robot/subsystems/Arm.java index 8cfcf9b..f42de1d 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/Arm.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/Arm.java @@ -1,6 +1,7 @@ package org.usfirst.frc.team3501.robot.subsystems; import org.usfirst.frc.team3501.robot.RobotMap; +import org.usfirst.frc.team3501.robot.commands.MoveArm; import edu.wpi.first.wpilibj.CANJaguar; import edu.wpi.first.wpilibj.command.Subsystem; @@ -19,6 +20,10 @@ public class Arm extends Subsystem { right.set(speed); } + public void setFromJoystick(double speed) { + set(getSpeedFromJoystick(speed)); + } + public void moveLeft(double speed) { left.set(speed); right.set(0); @@ -36,6 +41,7 @@ public class Arm extends Subsystem { return speed; } - public void initDefaultCommand() {} + public void initDefaultCommand() { + setDefaultCommand(new MoveArm()); + } } - diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Drivetrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/Drivetrain.java index 5dd39ab..72428e4 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/Drivetrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/Drivetrain.java @@ -51,4 +51,3 @@ public class Drivetrain extends Subsystem { setDefaultCommand(new DriveWithJoysticks()); } } - diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Pneumatics.java b/src/org/usfirst/frc/team3501/robot/subsystems/Pneumatics.java index 8a831d5..270758e 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/Pneumatics.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/Pneumatics.java @@ -21,4 +21,3 @@ public class Pneumatics extends Subsystem { public void initDefaultCommand() {} } -