From b26407834f93b26caa7617d3cfcdd7a39c732818 Mon Sep 17 00:00:00 2001 From: Logan Howard Date: Fri, 17 Apr 2015 21:40:25 -0700 Subject: [PATCH] big refactor of command/commandbase/commandgroup. also add auton read from file. --- auton_times_and_speeds.conf | 4 + .../usfirst/frc/team3501/robot/AutonData.java | 98 +++++++++++++++++++ src/org/usfirst/frc/team3501/robot/Robot.java | 19 ++-- .../usfirst/frc/team3501/robot/RobotMap.java | 5 - .../team3501/robot/autons/DriveOverStep.java | 9 +- .../team3501/robot/autons/DrivePastStep.java | 9 +- .../robot/autons/PickUpContainer.java | 7 +- .../team3501/robot/commands/CloseClaw.java | 2 +- .../frc/team3501/robot/commands/Command.java | 24 +++++ .../team3501/robot/commands/CommandBase.java | 40 ++------ .../team3501/robot/commands/CommandGroup.java | 10 ++ .../frc/team3501/robot/commands/DriveFor.java | 2 +- .../robot/commands/DriveWithJoysticks.java | 2 +- .../frc/team3501/robot/commands/MoveArm.java | 2 +- .../team3501/robot/commands/MoveArmFor.java | 2 +- .../frc/team3501/robot/commands/OpenClaw.java | 2 +- .../robot/commands/TensionLeftWinch.java | 2 +- .../robot/commands/TensionRightWinch.java | 2 +- .../team3501/robot/commands/ToggleClaw.java | 2 +- .../robot/commands/TurnOffCompressor.java | 2 +- .../robot/commands/TurnOnCompressor.java | 2 +- 21 files changed, 178 insertions(+), 69 deletions(-) create mode 100644 auton_times_and_speeds.conf create mode 100644 src/org/usfirst/frc/team3501/robot/AutonData.java create mode 100644 src/org/usfirst/frc/team3501/robot/commands/Command.java create mode 100644 src/org/usfirst/frc/team3501/robot/commands/CommandGroup.java diff --git a/auton_times_and_speeds.conf b/auton_times_and_speeds.conf new file mode 100644 index 0000000..f429f3f --- /dev/null +++ b/auton_times_and_speeds.conf @@ -0,0 +1,4 @@ +# format is: `speed time name`. speed can be negative for reverse motion. +0.7 1.2 drive_over_step +0.5 1.5 drive_past_step +0.5 1.4 pickup_container diff --git a/src/org/usfirst/frc/team3501/robot/AutonData.java b/src/org/usfirst/frc/team3501/robot/AutonData.java new file mode 100644 index 0000000..a643b06 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/AutonData.java @@ -0,0 +1,98 @@ +package org.usfirst.frc.team3501.robot; + +import java.io.BufferedReader; +import java.io.FileReader; +import java.io.IOException; +import java.util.Arrays; +import java.util.HashMap; + +public class AutonData { + + HashMap speeds; + HashMap times; + + public AutonData() { + speeds = new HashMap(); + times = new HashMap(); + + populate(); + } + + public double getSpeed(String key) { + Double ret = speeds.get(key); + + return (ret != null) ? ret : 0; + } + + public double getTime(String key) { + Double ret = times.get(key); + + return (ret != null) ? ret : 0; + } + + public void update() { + speeds.clear(); + times.clear(); + + populate(); + } + + private void populate() { + String file; + + try { + file = readConfigFile(); + } catch (IOException e) { + e.printStackTrace(); + populateDefaults(); + return; + } + + try { + Arrays.stream(file.split("\n")) + .map(line -> line.split(" ")) + .forEach((action) -> { + double speed = Double.parseDouble(action[0]); + double time = Double.parseDouble(action[1]); + String name = action[2]; + + speeds.put(name, speed); + times.put(name, time); + }); + } catch (Exception e) { + e.printStackTrace(); + populateDefaults(); + } + } + + private void populateDefaults() { + speeds.clear(); + times.clear(); + + speeds.put("drive_over_step", 0.7); + speeds.put("drive_past_step", 0.5); + speeds.put("pickup_container", 0.5); + + times.put("drive_over_step", 1.2); + times.put("drive_past_step", 1.5); + times.put("pickup_container", 1.4); + } + + private String readConfigFile() throws IOException { + BufferedReader in = new BufferedReader(new FileReader( + "auton_times_and_speeds.conf")); + + StringBuilder sb = new StringBuilder(); + + in.readLine(); // get rid of first line + + String curLine; + while ((curLine = in.readLine()) != null) + sb.append(curLine + "\n"); + String finalString = sb.toString(); + + in.close(); + + return finalString; + } +} diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index b99f059..532d254 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -22,8 +22,9 @@ public class Robot extends IterativeRobot { public static OI oi; - private SendableChooser autoChooser; + public static AutonData autonData; + private SendableChooser autonChooser; private Command autonomousCommand; public void robotInit() { @@ -35,6 +36,8 @@ public class Robot extends IterativeRobot { pneumatics = new Pneumatics(); + autonData = new AutonData(); + chooseAuto(); } @@ -45,7 +48,9 @@ public class Robot extends IterativeRobot { public void autonomousInit() { schedule(new TurnOnCompressor()); - autonomousCommand = (Command) autoChooser.getSelected(); + autonData.update(); + + autonomousCommand = (Command) autonChooser.getSelected(); autonomousCommand.start(); } @@ -68,13 +73,13 @@ public class Robot extends IterativeRobot { } private void chooseAuto() { - autoChooser = new SendableChooser(); + autonChooser = new SendableChooser(); - autoChooser.addDefault("Pick up container", new ContainerOverStep()); - autoChooser.addObject("Drive over step", new DriveOverStep()); - autoChooser.addObject("Drive past step", new DrivePastStep()); + autonChooser.addDefault("Pick up container", new ContainerOverStep()); + autonChooser.addObject("Drive over step", new DriveOverStep()); + autonChooser.addObject("Drive past step", new DrivePastStep()); - SmartDashboard.putData("Auto Mode", autoChooser); + SmartDashboard.putData("Auto Mode", autonChooser); } private void schedule(Command c) { diff --git a/src/org/usfirst/frc/team3501/robot/RobotMap.java b/src/org/usfirst/frc/team3501/robot/RobotMap.java index 2f2da69..37dad27 100644 --- a/src/org/usfirst/frc/team3501/robot/RobotMap.java +++ b/src/org/usfirst/frc/team3501/robot/RobotMap.java @@ -25,9 +25,4 @@ public class RobotMap { public static final int CLAW_FORWARD_CHANNEL = 0, CLAW_REVERSE_CHANNEL = 1; public static final Value OPEN = Value.kForward, CLOSED = Value.kReverse; - - // 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, - PICKUP_TIME = 1.4, PICKUP_SPEED = 0.5; } diff --git a/src/org/usfirst/frc/team3501/robot/autons/DriveOverStep.java b/src/org/usfirst/frc/team3501/robot/autons/DriveOverStep.java index f70b3d2..224bc9c 100644 --- a/src/org/usfirst/frc/team3501/robot/autons/DriveOverStep.java +++ b/src/org/usfirst/frc/team3501/robot/autons/DriveOverStep.java @@ -1,9 +1,8 @@ package org.usfirst.frc.team3501.robot.autons; -import org.usfirst.frc.team3501.robot.RobotMap; -import org.usfirst.frc.team3501.robot.commands.CommandBase; +import org.usfirst.frc.team3501.robot.commands.Command; -public class DriveOverStep extends CommandBase { +public class DriveOverStep extends Command { private double speed; @@ -11,8 +10,8 @@ public class DriveOverStep extends CommandBase { super("DriveOverStep"); requires(drivetrain); - setTimeout(RobotMap.OVER_STEP_TIME); - speed = RobotMap.OVER_STEP_SPEED; + setTimeout(autonData.getTime("drive_over_step")); + speed = autonData.getSpeed("drive_over_step"); } // TODO: this is an ugly "solution" diff --git a/src/org/usfirst/frc/team3501/robot/autons/DrivePastStep.java b/src/org/usfirst/frc/team3501/robot/autons/DrivePastStep.java index d3105f8..f459750 100644 --- a/src/org/usfirst/frc/team3501/robot/autons/DrivePastStep.java +++ b/src/org/usfirst/frc/team3501/robot/autons/DrivePastStep.java @@ -1,9 +1,8 @@ package org.usfirst.frc.team3501.robot.autons; -import org.usfirst.frc.team3501.robot.RobotMap; -import org.usfirst.frc.team3501.robot.commands.CommandBase; +import org.usfirst.frc.team3501.robot.commands.Command; -public class DrivePastStep extends CommandBase { +public class DrivePastStep extends Command { private double speed; @@ -11,8 +10,8 @@ public class DrivePastStep extends CommandBase { super("DrivePastStep"); requires(drivetrain); - setTimeout(RobotMap.PAST_STEP_TIME); - this.speed = RobotMap.PAST_STEP_SPEED; + setTimeout(autonData.getTime("drive_past_step")); + speed = autonData.getSpeed("drive_past_step"); } protected void execute() { diff --git a/src/org/usfirst/frc/team3501/robot/autons/PickUpContainer.java b/src/org/usfirst/frc/team3501/robot/autons/PickUpContainer.java index b930f12..52fb2b4 100644 --- a/src/org/usfirst/frc/team3501/robot/autons/PickUpContainer.java +++ b/src/org/usfirst/frc/team3501/robot/autons/PickUpContainer.java @@ -1,11 +1,8 @@ package org.usfirst.frc.team3501.robot.autons; import org.usfirst.frc.team3501.robot.Robot; -import org.usfirst.frc.team3501.robot.RobotMap; import org.usfirst.frc.team3501.robot.commands.*; -import edu.wpi.first.wpilibj.command.CommandGroup; - public class PickUpContainer extends CommandGroup { public PickUpContainer() { @@ -19,6 +16,8 @@ public class PickUpContainer extends CommandGroup { private void queueCommands() { addSequential(new CloseClaw()); - addSequential(new MoveArmFor(RobotMap.PICKUP_TIME, RobotMap.PICKUP_SPEED)); + addSequential(new MoveArmFor( + Robot.autonData.getTime("pickup_container"), + Robot.autonData.getSpeed("pickup_container"))); } } diff --git a/src/org/usfirst/frc/team3501/robot/commands/CloseClaw.java b/src/org/usfirst/frc/team3501/robot/commands/CloseClaw.java index 3cf7125..d4b1199 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/CloseClaw.java +++ b/src/org/usfirst/frc/team3501/robot/commands/CloseClaw.java @@ -1,6 +1,6 @@ package org.usfirst.frc.team3501.robot.commands; -public class CloseClaw extends CommandBase { +public class CloseClaw extends Command { public CloseClaw() { super("CloseClaw"); diff --git a/src/org/usfirst/frc/team3501/robot/commands/Command.java b/src/org/usfirst/frc/team3501/robot/commands/Command.java new file mode 100644 index 0000000..42c58d6 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commands/Command.java @@ -0,0 +1,24 @@ +package org.usfirst.frc.team3501.robot.commands; + +import edu.wpi.first.wpilibj.command.Scheduler; + +public abstract class Command + extends edu.wpi.first.wpilibj.command.Command + implements CommandBase { + + public Command(String commandName) { + super(commandName); + } + + protected void schedule(Command c) { + Scheduler.getInstance().add(c); + } + + protected void initialize() {} + + protected void execute() {} + + protected void end() {} + + protected void interrupted() {} +} diff --git a/src/org/usfirst/frc/team3501/robot/commands/CommandBase.java b/src/org/usfirst/frc/team3501/robot/commands/CommandBase.java index 5046704..3a5044e 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/CommandBase.java +++ b/src/org/usfirst/frc/team3501/robot/commands/CommandBase.java @@ -1,43 +1,19 @@ package org.usfirst.frc.team3501.robot.commands; +import org.usfirst.frc.team3501.robot.AutonData; import org.usfirst.frc.team3501.robot.OI; import org.usfirst.frc.team3501.robot.Robot; import org.usfirst.frc.team3501.robot.subsystems.*; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.command.Scheduler; +public interface CommandBase { -public abstract class CommandBase extends Command { + final static OI oi = Robot.oi; - protected static OI oi; + final static AutonData autonData = Robot.autonData; - protected static Drivetrain drivetrain; - protected static Arm arm; - protected static Claw claw; + final static Drivetrain drivetrain = Robot.drivetrain; + final static Arm arm = Robot.arm; + final static Claw claw = Robot.claw; - protected static Pneumatics pneumatics; - - public CommandBase(String commandName) { - super(commandName); - - oi = Robot.oi; - - drivetrain = Robot.drivetrain; - arm = Robot.arm; - claw = Robot.claw; - - pneumatics = Robot.pneumatics; - } - - protected void schedule(Command c) { - Scheduler.getInstance().add(c); - } - - protected void initialize() {} - - protected void execute() {} - - protected void end() {} - - protected void interrupted() {} + final static Pneumatics pneumatics = Robot.pneumatics; } diff --git a/src/org/usfirst/frc/team3501/robot/commands/CommandGroup.java b/src/org/usfirst/frc/team3501/robot/commands/CommandGroup.java new file mode 100644 index 0000000..fcb7d26 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commands/CommandGroup.java @@ -0,0 +1,10 @@ +package org.usfirst.frc.team3501.robot.commands; + +public class CommandGroup + extends edu.wpi.first.wpilibj.command.CommandGroup + implements CommandBase { + + public CommandGroup(String commandGroupName) { + super(commandGroupName); + } +} diff --git a/src/org/usfirst/frc/team3501/robot/commands/DriveFor.java b/src/org/usfirst/frc/team3501/robot/commands/DriveFor.java index c1ca507..295834b 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/DriveFor.java +++ b/src/org/usfirst/frc/team3501/robot/commands/DriveFor.java @@ -1,6 +1,6 @@ package org.usfirst.frc.team3501.robot.commands; -public class DriveFor extends CommandBase { +public class DriveFor extends Command { private double speed; diff --git a/src/org/usfirst/frc/team3501/robot/commands/DriveWithJoysticks.java b/src/org/usfirst/frc/team3501/robot/commands/DriveWithJoysticks.java index 338e01e..cf5446a 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/DriveWithJoysticks.java +++ b/src/org/usfirst/frc/team3501/robot/commands/DriveWithJoysticks.java @@ -1,6 +1,6 @@ package org.usfirst.frc.team3501.robot.commands; -public class DriveWithJoysticks extends CommandBase { +public class DriveWithJoysticks extends Command { public DriveWithJoysticks() { super("DriveWithJoysticks"); diff --git a/src/org/usfirst/frc/team3501/robot/commands/MoveArm.java b/src/org/usfirst/frc/team3501/robot/commands/MoveArm.java index 2354b3d..e209771 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/MoveArm.java +++ b/src/org/usfirst/frc/team3501/robot/commands/MoveArm.java @@ -1,6 +1,6 @@ package org.usfirst.frc.team3501.robot.commands; -public class MoveArm extends CommandBase { +public class MoveArm extends Command { public MoveArm() { super("MoveArm"); diff --git a/src/org/usfirst/frc/team3501/robot/commands/MoveArmFor.java b/src/org/usfirst/frc/team3501/robot/commands/MoveArmFor.java index 7f475f1..b9f3bc7 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/MoveArmFor.java +++ b/src/org/usfirst/frc/team3501/robot/commands/MoveArmFor.java @@ -1,6 +1,6 @@ package org.usfirst.frc.team3501.robot.commands; -public class MoveArmFor extends CommandBase { +public class MoveArmFor extends Command { private double speed; diff --git a/src/org/usfirst/frc/team3501/robot/commands/OpenClaw.java b/src/org/usfirst/frc/team3501/robot/commands/OpenClaw.java index a3aa3a2..6adb5a7 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/OpenClaw.java +++ b/src/org/usfirst/frc/team3501/robot/commands/OpenClaw.java @@ -1,6 +1,6 @@ package org.usfirst.frc.team3501.robot.commands; -public class OpenClaw extends CommandBase { +public class OpenClaw extends Command { public OpenClaw() { super("OpenClaw"); diff --git a/src/org/usfirst/frc/team3501/robot/commands/TensionLeftWinch.java b/src/org/usfirst/frc/team3501/robot/commands/TensionLeftWinch.java index 4d9712c..b915896 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/TensionLeftWinch.java +++ b/src/org/usfirst/frc/team3501/robot/commands/TensionLeftWinch.java @@ -1,6 +1,6 @@ package org.usfirst.frc.team3501.robot.commands; -public class TensionLeftWinch extends CommandBase { +public class TensionLeftWinch extends Command { private double speed; diff --git a/src/org/usfirst/frc/team3501/robot/commands/TensionRightWinch.java b/src/org/usfirst/frc/team3501/robot/commands/TensionRightWinch.java index 1bf68cf..30dfb10 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/TensionRightWinch.java +++ b/src/org/usfirst/frc/team3501/robot/commands/TensionRightWinch.java @@ -1,6 +1,6 @@ package org.usfirst.frc.team3501.robot.commands; -public class TensionRightWinch extends CommandBase { +public class TensionRightWinch extends Command { private double speed; diff --git a/src/org/usfirst/frc/team3501/robot/commands/ToggleClaw.java b/src/org/usfirst/frc/team3501/robot/commands/ToggleClaw.java index f1afa16..b7e7b52 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/ToggleClaw.java +++ b/src/org/usfirst/frc/team3501/robot/commands/ToggleClaw.java @@ -1,6 +1,6 @@ package org.usfirst.frc.team3501.robot.commands; -public class ToggleClaw extends CommandBase { +public class ToggleClaw extends Command { public ToggleClaw() { super("ToggleClaw"); diff --git a/src/org/usfirst/frc/team3501/robot/commands/TurnOffCompressor.java b/src/org/usfirst/frc/team3501/robot/commands/TurnOffCompressor.java index dbb78c9..85838f9 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/TurnOffCompressor.java +++ b/src/org/usfirst/frc/team3501/robot/commands/TurnOffCompressor.java @@ -1,6 +1,6 @@ package org.usfirst.frc.team3501.robot.commands; -public class TurnOffCompressor extends CommandBase { +public class TurnOffCompressor extends Command { public TurnOffCompressor() { super("TurnOffCompressor"); diff --git a/src/org/usfirst/frc/team3501/robot/commands/TurnOnCompressor.java b/src/org/usfirst/frc/team3501/robot/commands/TurnOnCompressor.java index 74d0fea..49853f2 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/TurnOnCompressor.java +++ b/src/org/usfirst/frc/team3501/robot/commands/TurnOnCompressor.java @@ -1,6 +1,6 @@ package org.usfirst.frc.team3501.robot.commands; -public class TurnOnCompressor extends CommandBase { +public class TurnOnCompressor extends Command { public TurnOnCompressor() { super("TurnOnCompressor"); -- 2.30.2