From a728e1dda3843528ed26ec6db2a931d444dc4adf Mon Sep 17 00:00:00 2001 From: mayabartels Date: Sat, 2 Feb 2019 15:02:19 -0800 Subject: [PATCH] Added commands for grabbing ball and hatch out of robot --- .../frc4388/robot/commands/FlipIntake.java | 53 +++++++++++++++++++ .../robot/commands/GrabBallOutOfRobot.java | 41 +++++++------- .../robot/commands/GrabHatchOffWall.java | 36 +++++++++++++ .../commands/GrabHatchOutOfFloorIntake.java | 37 +++++++++++++ .../robot/commands/RunWristMotorJumpBar.java | 53 +++++++++++++++++++ ...WristAvoidBar.java => StopWristMotor.java} | 24 ++++++--- .../frc4388/robot/subsystems/Wrist.java | 14 +++++ 7 files changed, 230 insertions(+), 28 deletions(-) create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/FlipIntake.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOffWall.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOutOfFloorIntake.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/RunWristMotorJumpBar.java rename 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/{WristAvoidBar.java => StopWristMotor.java} (77%) diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/FlipIntake.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/FlipIntake.java new file mode 100644 index 0000000..fc232a9 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/FlipIntake.java @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.*; + +import edu.wpi.first.wpilibj.command.Command; + +public class FlipIntake extends Command +{ + public FlipIntake() + { + requires(Robot.wrist); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() + { + Robot.wrist.controlPIDFlipIntake(); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() + { + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() + { + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() + { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() + { + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java index 5392859..6aa12c0 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java @@ -8,33 +8,34 @@ package org.usfirst.frc4388.robot.commands; import org.usfirst.frc4388.robot.Robot; -import edu.wpi.first.wpilibj.command.CommandGroup; -public class GrabBallOutOfRobot extends CommandGroup { - /** - * Add your docs here. - */ +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.WaitCommand; + +/** +* Add your docs here. +*/ + +public class GrabBallOutOfRobot extends CommandGroup +{ public GrabBallOutOfRobot() { + //Add rest of sequentials for this command group + + addSequential(new FlipIntake()); + //Move Arm until bar jump angle + + //Jump bar in robot if(Robot.wrist.armAngleDegrees <= Robot.wrist.jumpBarArmAngle && Robot.wrist.armAngleDegrees >= Robot.wrist.jumpBarArmAngle) { - + addParallel(new WaitCommand(2)); + addSequential(new RunWristMotorJumpBar()); + addSequential(new StopWristMotor()); } - // Add Commands here: - // e.g. addSequential(new Command1()); - // addSequential(new Command2()); - // these will run in order. - // To run multiple commands at the same time, - // use addParallel() - // e.g. addParallel(new Command1()); - // addSequential(new Command2()); - // Command1 and Command2 will run in parallel. + //continue arm movement until ball is in intake ///SENSOR for ball in robot - // A command group will require all of the subsystems that each member - // would require. - // e.g. if Command1 requires chassis, and Command2 requires arm, - // a CommandGroup containing them would require both the chassis and the - // arm. + //Move ball out of robot to 360 degrees + //move arm and wrist in parallel } } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOffWall.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOffWall.java new file mode 100644 index 0000000..d7f0377 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOffWall.java @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.usfirst.frc4388.robot.commands; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +/** +* Add your docs here. +*/ + +public class GrabHatchOffWall extends CommandGroup +{ + public GrabHatchOffWall() { + // Add Commands here: + // e.g. addSequential(new Command1()); + // addSequential(new Command2()); + // these will run in order. + + // To run multiple commands at the same time, + // use addParallel() + // e.g. addParallel(new Command1()); + // addSequential(new Command2()); + // Command1 and Command2 will run in parallel. + + // A command group will require all of the subsystems that each member + // would require. + // e.g. if Command1 requires chassis, and Command2 requires arm, + // a CommandGroup containing them would require both the chassis and the + // arm. + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOutOfFloorIntake.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOutOfFloorIntake.java new file mode 100644 index 0000000..b6c100c --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOutOfFloorIntake.java @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.usfirst.frc4388.robot.commands; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +/** +* Add your docs here. +*/ + +public class GrabHatchOutOfFloorIntake extends CommandGroup +{ + public GrabHatchOutOfFloorIntake() + { + // Add Commands here: + // e.g. addSequential(new Command1()); + // addSequential(new Command2()); + // these will run in order. + + // To run multiple commands at the same time, + // use addParallel() + // e.g. addParallel(new Command1()); + // addSequential(new Command2()); + // Command1 and Command2 will run in parallel. + + // A command group will require all of the subsystems that each member + // would require. + // e.g. if Command1 requires chassis, and Command2 requires arm, + // a CommandGroup containing them would require both the chassis and the + // arm. + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/RunWristMotorJumpBar.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/RunWristMotorJumpBar.java new file mode 100644 index 0000000..6fe8b19 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/RunWristMotorJumpBar.java @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.*; + +import edu.wpi.first.wpilibj.command.Command; + +public class RunWristMotorJumpBar extends Command +{ + public RunWristMotorJumpBar() + { + requires(Robot.wrist); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() + { + Robot.wrist.jumpBar(); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() + { + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() + { + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() + { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() + { + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristAvoidBar.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StopWristMotor.java similarity index 77% rename from 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristAvoidBar.java rename to 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StopWristMotor.java index 7e919ce..164e14f 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristAvoidBar.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StopWristMotor.java @@ -7,33 +7,41 @@ package org.usfirst.frc4388.robot.commands; +import org.usfirst.frc4388.robot.*; + import edu.wpi.first.wpilibj.command.Command; -public class WristAvoidBar extends Command { - public WristAvoidBar() { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); +public class StopWristMotor extends Command +{ + public StopWristMotor() + { + requires(Robot.wrist); } // Called just before this Command runs the first time @Override - protected void initialize() { + protected void initialize() + { + Robot.wrist.stopMotor(); } // Called repeatedly when this Command is scheduled to run @Override - protected void execute() { + protected void execute() + { } // Make this return true when this Command no longer needs to run execute() @Override - protected boolean isFinished() { + protected boolean isFinished() + { return false; } // Called once after isFinished returns true @Override - protected void end() { + protected void end() + { } // Called when another command which requires one or more of the same diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java index 6fc8e65..5f3b0b1 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java @@ -26,6 +26,7 @@ import com.ctre.phoenix.motorcontrol.LimitSwitchSource; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.SensorCollection; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import edu.wpi.first.wpilibj.command.WaitCommand; import com.ctre.phoenix.motorcontrol.ControlMode; @@ -105,6 +106,19 @@ public class Wrist extends Subsystem } } + //Jump bar by putting power to the motors for a specific amount of time + //Jump bar output + public void jumpBar() + { + wristRight.set(0.8); + } + + //Stop wrist motor + public void stopMotor() + { + wristRight.set(0); + } + //Method for setting the control mode for the wrist private synchronized void setWristControlMode(WristControlMode controlMode) {