From 3755d5f88ff9e0f4362ef763073532f23622a2df Mon Sep 17 00:00:00 2001 From: Adam Long Date: Wed, 21 Sep 2016 19:32:41 +0000 Subject: [PATCH] added more auto routines --- src/org/usfirst/frc/team2059/robot/Robot.java | 2 +- .../commands/autonomous/AutoDriveTime.java | 2 +- .../commands/autonomous/AutoResetLower.java | 37 ++++++++++++++++ .../autonomous/AutoSetArmPosition.java | 42 +++++++++++++++++++ .../autonomous/RoutineDefenseTime.java | 13 ++++++ .../commands/autonomous/RoutineDriveTime.java | 1 + .../commands/shooter/SetArmPosition.java | 3 +- .../team2059/robot/subsystems/MainArm.java | 10 ++--- 8 files changed, 101 insertions(+), 9 deletions(-) create mode 100644 src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoResetLower.java create mode 100644 src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoSetArmPosition.java create mode 100644 src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDefenseTime.java diff --git a/src/org/usfirst/frc/team2059/robot/Robot.java b/src/org/usfirst/frc/team2059/robot/Robot.java index 96bea3a..981210e 100644 --- a/src/org/usfirst/frc/team2059/robot/Robot.java +++ b/src/org/usfirst/frc/team2059/robot/Robot.java @@ -17,7 +17,7 @@ public class Robot extends IterativeRobot { chooser = new SendableChooser(); chooser.addDefault("Time based drive", new RoutineDriveTime()); - chooser.addObject("Distance based drive", new RoutineDriveTime()); + chooser.addObject("Time based defense", new RoutineDefenseTime()); SmartDashboard.putData("Auto mode", chooser); SmartDashboard.putData("MainArm", CommandBase.mainArm.getPIDController()); diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveTime.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveTime.java index 55c357f..a7a6788 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveTime.java +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveTime.java @@ -11,7 +11,7 @@ public class AutoDriveTime extends CommandBase { protected void initialize() { } protected void execute() { - driveBase.driveArcade(0,power,0,0); + driveBase.driveArcade(0,-power,0,0); } protected void end() { driveBase.stopDriving(); diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoResetLower.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoResetLower.java new file mode 100644 index 0000000..c2b1ed5 --- /dev/null +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoResetLower.java @@ -0,0 +1,37 @@ +package org.usfirst.frc.team2059.robot.commands.autonomous; +import org.usfirst.frc.team2059.robot.commands.CommandBase; +import org.usfirst.frc.team2059.robot.Robot; +/** + * + */ +public class AutoResetLower extends CommandBase { + double speed; + public AutoResetLower(double s) { + requires(mainArm); + speed = s; + } + // Called just before this Command runs the first time + protected void initialize() { + } + // Called repeatedly when this Command is scheduled to run + protected void execute() { + System.out.println("test"); + mainArm.disable(); + mainArm.resetLower(speed); + } + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + // Stop when bottom limit switch is hit + return mainArm.getBottomPressed(); + } + // Called once after isFinished returns true + protected void end() { + mainArm.moveArm(0); + } + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + end(); + } +} +// vim: sw=2:ts=2:sts=2 diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoSetArmPosition.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoSetArmPosition.java new file mode 100644 index 0000000..2bbea7f --- /dev/null +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoSetArmPosition.java @@ -0,0 +1,42 @@ +package org.usfirst.frc.team2059.robot.commands.autonomous; +import org.usfirst.frc.team2059.robot.commands.CommandBase; +import org.usfirst.frc.team2059.robot.Robot; +import org.usfirst.frc.team2059.robot.RobotMap; +/** + * + */ +public class AutoSetArmPosition extends CommandBase { + double pos; + public AutoSetArmPosition(double p) { + requires(mainArm); + pos = p; + } + // Called just before this Command runs the first time + protected void initialize() { + } + // Called repeatedly when this Command is scheduled to run + protected void execute() { + mainArm.enable(); + mainArm.setSetpoint(pos); + System.out.println(pos-mainArm.getDegrees()); + System.out.println(mainArm.getDegrees()); + } + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + // Stop when either limit switch is hit + //if(Math.abs(pos-mainArm.getDegrees()) <= 1){ + // return false; + //} + return false; + } + // Called once after isFinished returns true + protected void end() { + mainArm.disable(); + } + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + end(); + } +} +// vim: sw=2:ts=2:sts=2 diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDefenseTime.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDefenseTime.java new file mode 100644 index 0000000..b37f54b --- /dev/null +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDefenseTime.java @@ -0,0 +1,13 @@ +package org.usfirst.frc.team2059.robot.commands.autonomous; +import org.usfirst.frc.team2059.robot.commands.CommandBase; +import edu.wpi.first.wpilibj.command.CommandGroup; +import org.usfirst.frc.team2059.robot.Robot; +public class RoutineDefenseTime extends CommandGroup{ + public RoutineDefenseTime(){ + addSequential(new AutoResetLower(-1)); + addSequential(new AutoSetArmPosition(15)); + //Extend stop + addSequential(new AutoSetArmPosition(10)); + addSequential(new AutoDriveTime(2,.5)); + } +} diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveTime.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveTime.java index 0cf108d..5a66a7b 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveTime.java +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveTime.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj.command.CommandGroup; import org.usfirst.frc.team2059.robot.Robot; public class RoutineDriveTime extends CommandGroup{ public RoutineDriveTime(){ + addSequential(new AutoResetLower(-1)); addSequential(new AutoDriveTime(2,.5)); } } diff --git a/src/org/usfirst/frc/team2059/robot/commands/shooter/SetArmPosition.java b/src/org/usfirst/frc/team2059/robot/commands/shooter/SetArmPosition.java index 41d3fbd..22bd6bb 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/shooter/SetArmPosition.java +++ b/src/org/usfirst/frc/team2059/robot/commands/shooter/SetArmPosition.java @@ -28,7 +28,8 @@ public class SetArmPosition extends CommandBase { // Make this return true when this Command no longer needs to run execute() protected boolean isFinished() { // Stop when either limit switch is hit - return mainArm.getBottomPressed() || mainArm.getTopPressed(); + //return mainArm.getBottomPressed() || mainArm.getTopPressed(); + return false; } // Called once after isFinished returns true protected void end() { diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java index 7c9c398..f0e42e4 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java @@ -41,11 +41,9 @@ public class MainArm extends PIDSubsystem { } public void resetLower(double speed) { if (getBottomPressed()) { - System.out.println("PRESSDE"); moveArm(0); return; } else { - System.out.println("not pressed"); moveArm(speed); } } @@ -66,7 +64,7 @@ public class MainArm extends PIDSubsystem { return !limitSwitchTop.get(); } private double potToDegrees(double pot) { - System.out.println((pot - min) / (Math.abs(min - max) / 90)); + //System.out.println((pot - min) / (Math.abs(min - max) / 90)); return (pot - min) / (Math.abs(min - max) / 90); } /** @@ -75,9 +73,9 @@ public class MainArm extends PIDSubsystem { */ private boolean calibrate() { // It can't be calibrated if the limit swithces are disabled - if (!SmartDashboard.getBoolean("UseLimitSwitches")) { - return false; - } + //if (!SmartDashboard.getBoolean("UseLimitSwitches")) { + // return false; + //} if (getBottomPressed()) { System.out.println("Calibrating bottom to: " + getRaw()); min = getRaw();