diff --git a/RobotCode/Team100Robot/Team100_2019Workspace.code-workspace b/RobotCode/Team100Robot/Team100_2019Workspace.code-workspace new file mode 100644 index 0000000..15c77e1 --- /dev/null +++ b/RobotCode/Team100Robot/Team100_2019Workspace.code-workspace @@ -0,0 +1,10 @@ +{ + "folders": [ + { + "path": "." + } + ], + "settings": { + "java.configuration.updateBuildConfiguration": "automatic" + } +} \ No newline at end of file diff --git a/RobotCode/Team100Robot/build.gradle b/RobotCode/Team100Robot/build.gradle index 832d1ac..31def33 100644 --- a/RobotCode/Team100Robot/build.gradle +++ b/RobotCode/Team100Robot/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2019.2.1" + id "edu.wpi.first.GradleRIO" version "2019.3.2" } def ROBOT_MAIN_CLASS = "org.usfirst.frc100.Team100Robot.Main" diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/Constants.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/Constants.java index 268a409..8a497cc 100644 --- a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/Constants.java +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/Constants.java @@ -19,65 +19,112 @@ public class Constants { /* * CAN/PWM IDs */ - public static SerialPort.Port NAVX_COMM_PORT = SerialPort.Port.kUSB; - public static final int PCM_CANID = 0; - public static final int DRIVE_TRAIN_LEFT_MASTER_CANID = 0; - public static final int DRIVE_TRAIN_LEFT_FOLLOWER_CANID = 1; - public static final int DRIVE_TRAIN_RIGHT_MASTER_CANID = 15; - public static final int DRIVE_TRAIN_RIGHT_FOLLOWER_CANID = 14; - public static final int ELEVATOR_MASTER_CANID = 10; - public static final int ELEVATOR_FOLLOWER_CANID = 11; - public static final int ELEVATOR_CARRIAGE_SHOULDER_CANID = 2; - public static final int CARGO_PICKUP_TALON_CANID = 3; - public static final int CARGO_PICKUP_TILT_PWM = 6; - public static final int CARGO_PICKUP_ROLLER_PWM = 7; - public static final int HATCH_PICKUP_ROLLER_PWM = 8; - public static final int HATCH_PICKUP_TILT_CANID = 9; - public static final int CARGO_HATCH_SCORER_TOP_CANID = 4; - public static final int CARGO_HATCH_SCORER_BOTTOM_CANID = 5; - public static final int CLIMBER_MASTER_CANID = 12; - public static final int CLIMBER_FOLLOWER_CANID = 13; + public static SerialPort.Port NAVX_COMM_PORT = SerialPort.Port.kUSB; //Type: ENUM + public static final int PCM_CANID = 0; //Type: CAN ID + public static final int DRIVE_TRAIN_LEFT_MASTER_CANID = 0; //Type: CAN ID + public static final int DRIVE_TRAIN_LEFT_FOLLOWER_CANID = 3; //Type: CAN ID + public static final int DRIVE_TRAIN_RIGHT_MASTER_CANID = 15; //Type: CAN ID + public static final int DRIVE_TRAIN_RIGHT_FOLLOWER_CANID = 13; //Type: CAN ID + public static final int ELEVATOR_MASTER_CANID = 11; //Type: CAN ID + public static final int ELEVATOR_FOLLOWER_CANID = 9; //Type: CAN ID + public static final int ELEVATOR_CARRIAGE_SHOULDER_CANID = 1; //Type: CAN ID + public static final int CARGO_PICKUP_TILT_CANID = 2; //Type: CAN ID + public static final int CARGO_PICKUP_ROLLER1_PWM = 6; //Type: PWM ID + public static final int CARGO_PICKUP_ROLLER2_PWM = 7; //Type: PWM ID + public static final int HATCH_PICKUP_ROLLER_PWM = 8; //Type: PWM ID + public static final int HATCH_PICKUP_TILT_CANID = 10; //Type: CAN ID + public static final int CARGO_HATCH_SCORER_TOP_CANID = 5; //Type: CAN ID + public static final int CARGO_HATCH_SCORER_BOTTOM_CANID = 4; //Type: CAN ID + public static final int CLIMBER_MASTER_CANID = 14; //Type: CAN ID + public static final int CLIMBER_FOLLOWER_CANID = 12; //Type: CAN ID /* * PNEUMATICS SOLENOID PCM IDs */ - public static final int DRIVETRAIN_SHIFTER_PCMID = 0; - public static final int LOADING_STATION_INTAKE_PCMID = 1; - public static final int CARGO_GROUND_PICKUP_PCMID = 5; - public static final int HATCH_PICKUP_PCMID = 4; - public static final int CARGO_SCORER_PCMID = 3; - public static final int HATCH_SCORER_PCMID = 2; - public static final int CLIMBER_DEPLOY_PCMID = 6; + public static final int DRIVETRAIN_SHIFTER_PCMID = 5; //Type: PCM ID + public static final int LOADING_STATION_INTAKE_PCMID = 1; //Type: PCM ID + public static final int CARGO_GROUND_PICKUP_PCMID = 6; //Type: PCM ID + public static final int CARGO_GROUND_PICKUP2_PCMID = 7; //Type: PCM ID + public static final int HATCH_PICKUP_PCMID = 2; //Type: PCM ID + public static final int CARGO_SCORER_PCMID = 4; //Type: PCM ID DOESN'T EXIST + public static final int HATCH_SCORER_PCMID = 3; //Type: PCM ID + public static final int CLIMBER_DEPLOY_PCMID = 0; //Type: PCM ID /* * Drivetrain */ - public static final double DRIVE_TRAIN_MAX_MOTOR_OUTPUT = 0.8; - public static final boolean DRIVE_TRAIN_LEFT_MASTER_INVERT = false; - public static final InvertType DRIVE_TRAIN_LEFT_FOLLOWER_INVERT = InvertType.FollowMaster; - public static final boolean DRIVE_TRAIN_RIGHT_MASTER_INVERT = false; - public static final InvertType DRIVE_TRAIN_RIGHT_FOLLOWER_INVERT = InvertType.FollowMaster; - public static final boolean ARCADE_DRIVE_MODE = true; + public static final double DRIVE_TRAIN_MAX_MOTOR_OUTPUT = 0.8; //Type: Percent Output (-1-1) + public static final boolean DRIVE_TRAIN_LEFT_MASTER_INVERT = false; //Type: Boolean + public static final InvertType DRIVE_TRAIN_LEFT_FOLLOWER_INVERT = InvertType.FollowMaster; //Type: ENUM + public static final boolean DRIVE_TRAIN_RIGHT_MASTER_INVERT = false; //Type: Boolean + public static final InvertType DRIVE_TRAIN_RIGHT_FOLLOWER_INVERT = InvertType.FollowMaster; //Type: ENUM + public static final boolean ARCADE_DRIVE_MODE = true; //Type: Boolean //PID Rotation with NavX - public static final double DT_TURN_P = 0.007; - public static final double DT_TURN_I = 0.00004; - public static final double DT_TURN_D = 0.002; - public static final double DT_TURN_F = 0; - public static final double DT_TURN_MIN_ROTATION_ANGLE = -180; - public static final double DT_TURN_MAX_ROTATION_ANGLE = 180; - public static final double DT_TURN_MIN_OUTPUT = -1; - public static final double DT_TURN_MAX_OUTPUT = 1; - public static final double DT_TURN_ABSOLUTE_TOLERANCE = 1; - public static final double DT_TURN_MOTOR_OUTPUT = 0.5; + public static final double DT_TURN_P = 0.007; //Type: PIDF Double + public static final double DT_TURN_I = 0.00004; //Type: PIDF Double + public static final double DT_TURN_D = 0.002; //Type: PIDF Double + public static final double DT_TURN_F = 0; //Type: PIDF Double + public static final double DT_TURN_MIN_ROTATION_ANGLE = -180; //Type: Degrees + public static final double DT_TURN_MAX_ROTATION_ANGLE = 180; //Type: Degrees + public static final double DT_TURN_MIN_OUTPUT = -1; //Type: Percent Output (-1-1) + public static final double DT_TURN_MAX_OUTPUT = 1; //Type: Percent Output (-1-1) + public static final double DT_TURN_ABSOLUTE_TOLERANCE = 1; //Type: Degrees + public static final double DT_TURN_MOTOR_OUTPUT = 0.5; //Type: Percent Output (-1-1) /* * Elevator */ + /**Power to apply when going down during homing sequence... keep very small*/ + public static final double HOMING_GOING_DOWN_POWER = -.3; //Type: Percent Output (-1-1) + /**Power to apply when going up during homing sequence... keep very small */ + public static final double HOMING_GOING_UP_POWER = .5; //Type: Percent Output (-1-1) + /**Buffer in elevator target for acceptable destination */ + public static final int ELEVATOR_POSITION_BUFFER = 85; //Type: Encoder Ticks + + /** + * Elevator Timeout in Milliseconds + */ + public static final int ELEVATOR_MASTER_TIMEOUT = 10; //Type: Milliseconds + + + ////////////// PID + + /**Elevator PID P */ + public static final double ELEVATOR_KP = 2; //Type: PIDF Double + + /**Elevator PID I */ + public static final double ELEVATOR_KI = 0; //Type: PIDF Double + + /**Elevator PID D */ + public static final double ELEVATOR_KD = 0; //Type: PIDF Double + + /**Elevator PID F */ + public static final double ELEVATOR_KF = 1; //Type: PIDF Double + + + /**Conversion factor from Inches to Encoder ticks + * For elevator control + */ + public static final int ELEVATOR_INCH_TO_ENCODER_CONVERSION_FACTION = 86; //Type: Encoder Ticks + + public static final double ELEVATOR_START_HEIGHT_IN_INCHES = 16.5; //Type: Inches + public static final double ELEVATOR_MAX_HEIGHT_IN_INCHES = 78.5; //Type: Inches /* * Elevator Carriage */ + /* + * Shoulder + */ + public static final int SHOULDER_MASTER_TIMEOUT = 10; //Type: Milliseconds + + public static final double SHOULDER_KP = 0.001; //Type: PIDF Double + public static final double SHOULDER_KI = 0; //Type: PIDF Double + public static final double SHOULDER_KD = 0; //Type: PIDF Double + public static final double SHOULDER_KF = 0; //Type: PIDF Double + public static final int SHOULDER_TIMEOUT = 10; //Type: Milliseconds + public static final int SHOULDER_BUFFER = 10; //Type: Encoder Ticks /* * Cargo Floor Pickup */ @@ -85,8 +132,8 @@ public class Constants { /* * Hatch Floor Pickup */ - public static final double HATCH_PICKUP_INTAKE_SPEED = 0.6; - public static final double HATCH_PICKUP_TILT_SPEED = 0.2; + public static final double HATCH_PICKUP_INTAKE_SPEED = 0.6; //Type: Percent Output (-1-1) + public static final double HATCH_PICKUP_TILT_SPEED = 0.2; //Type: Percent Output (-1-1) public static final int HATCH_PICKUP_UPPER_LIMIT_SWITCH_SLOT = 0; public static final int HATCH_PICKUP_LOWER_LIMIT_SWITCH_SLOT = 1; @@ -98,4 +145,12 @@ public class Constants { * Climber */ + /* + * Cargo Manipulator + */ + public static final int CARGO_MANIPULATOR_TOP_TALONSRX_ID = 5; + public static final int CARGO_MANIPULATOR_BOTTOM_TALONSRX_ID = 4; + public static final double CARGO_MANIPULATOR_INTAKE_SPEED = 1; + public static final double CARGO_MANIPULATOR_OUTTAKE_SPEED = -1; + } diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/OI.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/OI.java index b1ce939..15aacfd 100644 --- a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/OI.java +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/OI.java @@ -12,6 +12,19 @@ package org.usfirst.frc100.Team100Robot; import org.usfirst.frc100.Team100Robot.commands.*; +import org.usfirst.frc100.Team100Robot.commands.CargoManipulator.CargoManipulatorOuttake; +import org.usfirst.frc100.Team100Robot.commands.Drivetrain.Drive; +import org.usfirst.frc100.Team100Robot.commands.Drivetrain.Shift.ShiftToHigh; +import org.usfirst.frc100.Team100Robot.commands.Drivetrain.Shift.ShiftToLow; +import org.usfirst.frc100.Team100Robot.commands.Elevator.ElevatorPageDown; +import org.usfirst.frc100.Team100Robot.commands.Elevator.ElevatorPageUp; +import org.usfirst.frc100.Team100Robot.commands.HatchManipulator.Bill.BillLower; +import org.usfirst.frc100.Team100Robot.commands.HatchManipulator.Bill.BillRaise; +import org.usfirst.frc100.Team100Robot.commands.HatchManipulator.Pusher.*; +import org.usfirst.frc100.Team100Robot.commands.IntakeArm.IntakeArmIntakeElement; +import org.usfirst.frc100.Team100Robot.commands.Shoulder.ShoulderDown; +import org.usfirst.frc100.Team100Robot.commands.Shoulder.ShoulderUp; + import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.buttons.JoystickButton; @@ -52,6 +65,7 @@ public class OI { private Joystick leftStick; private Joystick rightStick; private Joystick manipulatorControl; + private Joystick testingJS; private JoystickButton shiftLow; private JoystickButton shiftHigh; @@ -66,6 +80,7 @@ public class OI { private JoystickButton elevatorStageUp; private JoystickButton hatchRelease; private JoystickButton elevatorStageDown; + public OI() { @@ -75,16 +90,17 @@ public OI() { leftStick = new Joystick(0); rightStick = new Joystick(1); manipulatorControl = new Joystick(2); + testingJS = new Joystick(4); /* * Joystick 0 (Left Stick) */ spitCargo = new JoystickButton(leftStick, 1); - spitCargo.whenPressed(new SpitCargo()); + spitCargo.whileHeld(new CargoManipulatorOuttake()); shiftLow = new JoystickButton(leftStick, 3); - shiftLow.whenPressed(new LowGear()); + shiftLow.whenPressed(new ShiftToLow()); hatchUp = new JoystickButton(leftStick, 4); @@ -94,13 +110,13 @@ public OI() { intakeCargo = new JoystickButton(rightStick, 1); shiftHigh = new JoystickButton(rightStick, 3); - shiftHigh.whenPressed(new HighGear()); + shiftHigh.whenPressed(new ShiftToHigh()); cargoUp = new JoystickButton(rightStick, 4); - cargoUp.whenPressed(new CargoUp()); + cargoUp.whenPressed(new ShoulderUp()); cargoDown = new JoystickButton(rightStick, 5); - cargoDown.whenPressed(new CargoDown()); + cargoDown.whenPressed(new ShoulderDown()); //Joystick 2 (Manipulator Control) climberToggle = new JoystickButton(manipulatorControl, 4); @@ -114,6 +130,26 @@ public OI() { elevatorStageDown = new JoystickButton(manipulatorControl, 8); + + + elevatorStageUp.whenPressed(new ElevatorPageUp()); + elevatorStageDown.whenPressed(new ElevatorPageDown()); + intakeCargo.whileActive(new IntakeArmIntakeElement()); + + + /* + * Testing Joystick __(will not be used for comp)___ + */ + JoystickButton duckBillExtend = new JoystickButton(testingJS, 1); + duckBillExtend.whileActive(new BillRaise()); + JoystickButton duckBillRetract = new JoystickButton(testingJS,2); + duckBillRetract.whileActive(new BillLower()); + JoystickButton pusherExtend = new JoystickButton(testingJS, 3); + pusherExtend.whileActive(new ExtendPusher()); + JoystickButton pusherRetract = new JoystickButton(testingJS, 4); + pusherRetract.whileActive(new RetractPusher()); + + // SmartDashboard Buttons SmartDashboard.putData("Autonomous Command", new AutonomousCommand()); @@ -133,6 +169,9 @@ public Joystick getRightStick() { public Joystick getManipulatorControl() { return manipulatorControl; } + public Joystick getTestingJS(){ + return testingJS; + } } diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/Robot.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/Robot.java index 85b97c7..9b2305f 100644 --- a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/Robot.java +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/Robot.java @@ -18,6 +18,15 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.usfirst.frc100.Team100Robot.commands.*; +import org.usfirst.frc100.Team100Robot.commands.CargoManipulator.CargoManipulatorIntake; +import org.usfirst.frc100.Team100Robot.commands.CargoManipulator.CargoManipulatorOuttake; +import org.usfirst.frc100.Team100Robot.commands.Drivetrain.Shift.ShiftToHigh; +import org.usfirst.frc100.Team100Robot.commands.Drivetrain.Shift.ShiftToLow; +import org.usfirst.frc100.Team100Robot.commands.HatchManipulator.Bill.BillLower; +import org.usfirst.frc100.Team100Robot.commands.HatchManipulator.Bill.BillRaise; +import org.usfirst.frc100.Team100Robot.commands.HatchManipulator.Pusher.ExtendPusher; +import org.usfirst.frc100.Team100Robot.commands.HatchManipulator.Pusher.RetractPusher; +import org.usfirst.frc100.Team100Robot.commands.IntakeArm.IntakeArmIntakeElement; import org.usfirst.frc100.Team100Robot.subsystems.*; import com.kauailabs.navx.frc.*; @@ -44,6 +53,7 @@ public class Robot extends TimedRobot { public static Elevator elevator; public static double currentHeading; public static AHRS ahrs; + public static Manipulator manipulator; /** * This function is run when the robot is first started up and should be @@ -51,7 +61,7 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { - ahrs = new AHRS(Constants.NAVX_COMM_PORT); + // ahrs = new AHRS(Constants.NAVX_COMM_PORT); drivetrain = new Drivetrain(); shifter = new Shifter(); @@ -61,6 +71,7 @@ public void robotInit() { cargoPickup = new CargoPickup(); hatchPickup = new HatchPickup(); elevator = new Elevator(); + manipulator = new Manipulator(); // OI must be constructed after subsystems. If the OI creates Commands //(which it very likely will), subsystems are not guaranteed to be @@ -72,6 +83,21 @@ public void robotInit() { chooser.setDefaultOption("Autonomous Command", new AutonomousCommand()); SmartDashboard.putData("Auto mode", chooser); + + + + + + // Put Command Triggers + SmartDashboard.putData("CargoManipIntake",new CargoManipulatorIntake()); + SmartDashboard.putData("CargoManipOuttake",new CargoManipulatorOuttake()); + SmartDashboard.putData("DTShiftToHigh",new ShiftToHigh()); + SmartDashboard.putData("DTShiftToLow",new ShiftToLow()); + SmartDashboard.putData("BillRaise",new BillRaise()); + SmartDashboard.putData("BillLower",new BillLower()); + SmartDashboard.putData("PusherExtend",new ExtendPusher()); + SmartDashboard.putData("PusherRetract",new RetractPusher()); + SmartDashboard.putData("IntakeElement", new IntakeArmIntakeElement()); } /** @@ -119,7 +145,7 @@ public void teleopInit() { @Override public void teleopPeriodic() { Scheduler.getInstance().run(); - currentHeading = ahrs.getFusedHeading(); + //currentHeading = ahrs.getFusedHeading(); } public static double getCurrentHeading(){ diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/CargoDown.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/CargoDown.java index 7e9990d..f08d26f 100644 --- a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/CargoDown.java +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/CargoDown.java @@ -7,17 +7,21 @@ package org.usfirst.frc100.Team100Robot.commands; +import org.usfirst.frc100.Team100Robot.Robot; + import edu.wpi.first.wpilibj.command.Command; public class CargoDown extends Command { public CargoDown() { // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); + // eg. requires(Robot); + } // Called just before this Command runs the first time @Override protected void initialize() { + } // Called repeatedly when this Command is scheduled to run diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/CargoManipulator/CargoManipulatorDefault.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/CargoManipulator/CargoManipulatorDefault.java new file mode 100644 index 0000000..64f3f77 --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/CargoManipulator/CargoManipulatorDefault.java @@ -0,0 +1,47 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.CargoManipulator; + +import org.usfirst.frc100.Team100Robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class CargoManipulatorDefault extends Command { + public CargoManipulatorDefault() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.manipulator); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // 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/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/CargoManipulator/CargoManipulatorIntake.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/CargoManipulator/CargoManipulatorIntake.java new file mode 100644 index 0000000..b774228 --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/CargoManipulator/CargoManipulatorIntake.java @@ -0,0 +1,58 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.CargoManipulator; + +import com.ctre.phoenix.motorcontrol.ControlMode; + +import org.usfirst.frc100.Team100Robot.Constants; +import org.usfirst.frc100.Team100Robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class CargoManipulatorIntake extends Command { + private boolean done = false; + public CargoManipulatorIntake() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.manipulator); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + done = false; + Robot.manipulator.topRoller.set(ControlMode.PercentOutput,Constants.CARGO_MANIPULATOR_INTAKE_SPEED); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + if(Robot.manipulator.cargoSensor.get()){ + done = true; + } + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return done; + } + + // Called once after isFinished returns true + @Override + protected void end() { + Robot.manipulator.topRoller.set(ControlMode.PercentOutput,0); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } +} diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/CargoManipulator/CargoManipulatorOuttake.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/CargoManipulator/CargoManipulatorOuttake.java new file mode 100644 index 0000000..641301d --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/CargoManipulator/CargoManipulatorOuttake.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.frc100.Team100Robot.commands.CargoManipulator; + +import com.ctre.phoenix.motorcontrol.ControlMode; + +import org.usfirst.frc100.Team100Robot.Constants; +import org.usfirst.frc100.Team100Robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class CargoManipulatorOuttake extends Command { + public CargoManipulatorOuttake() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.manipulator); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.manipulator.topRoller.set(ControlMode.PercentOutput,Constants.CARGO_MANIPULATOR_OUTTAKE_SPEED); + } + + // 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() { + Robot.manipulator.topRoller.set(ControlMode.PercentOutput,0); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } +} diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Drive.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Drivetrain/Drive.java similarity index 95% rename from RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Drive.java rename to RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Drivetrain/Drive.java index 854583b..da1bf9e 100644 --- a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Drive.java +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Drivetrain/Drive.java @@ -9,7 +9,7 @@ // it from being updated in the future. -package org.usfirst.frc100.Team100Robot.commands; +package org.usfirst.frc100.Team100Robot.commands.Drivetrain; import org.usfirst.frc100.Team100Robot.Robot; diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Drivetrain/Shift/ShiftToHigh.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Drivetrain/Shift/ShiftToHigh.java new file mode 100644 index 0000000..e405763 --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Drivetrain/Shift/ShiftToHigh.java @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.Drivetrain.Shift; + +import org.usfirst.frc100.Team100Robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class ShiftToHigh extends Command { + public ShiftToHigh() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.drivetrain); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + System.out.println("ShiftToHigh"); + Robot.drivetrain.shift.set(true); + } + + // 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 true; + } + + // 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() { + end(); + } +} diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Drivetrain/Shift/ShiftToLow.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Drivetrain/Shift/ShiftToLow.java new file mode 100644 index 0000000..562f9ee --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Drivetrain/Shift/ShiftToLow.java @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.Drivetrain.Shift; + +import org.usfirst.frc100.Team100Robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class ShiftToLow extends Command { + public ShiftToLow() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.drivetrain); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + System.out.println("ShiftToLow"); + Robot.drivetrain.shift.set(false); + } + + // 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 true; + } + + // 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() { + end(); + } +} diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/ElevatorAtSetpoint.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/ElevatorAtSetpoint.java new file mode 100644 index 0000000..cfb1f1b --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/ElevatorAtSetpoint.java @@ -0,0 +1,64 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.Elevator; + +import com.ctre.phoenix.motorcontrol.ControlMode; + +import org.usfirst.frc100.Team100Robot.Robot; +import org.usfirst.frc100.Team100Robot.commands.Elevator.Homing.ElevatorHomingInit; +import org.usfirst.frc100.Team100Robot.subsystems.Elevator.States; + +import edu.wpi.first.wpilibj.command.Command; + +public class ElevatorAtSetpoint extends Command { + public ElevatorAtSetpoint() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.elevator); + + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + if(!Robot.elevator.homed){ + new ElevatorHomingInit().start(); + } + Robot.elevator.state = States.AT_SETPOINT; + if(Math.abs(Robot.oi.getManipulatorControl().getRawAxis(3)) > 0.2){ + new ElevatorTeleop().start(); + } + if(Robot.elevator.state == States.AT_SETPOINT){ + //Robot.elevator.elevatorMaster.set(ControlMode.Position,Robot.elevator.setpoint); + + } + } + + // 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/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/ElevatorMoveToSetpoint.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/ElevatorMoveToSetpoint.java new file mode 100644 index 0000000..ceb860e --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/ElevatorMoveToSetpoint.java @@ -0,0 +1,67 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.Elevator; + +import com.ctre.phoenix.motorcontrol.ControlMode; + +import org.usfirst.frc100.Team100Robot.Constants; +import org.usfirst.frc100.Team100Robot.Robot; +import org.usfirst.frc100.Team100Robot.subsystems.Elevator.States; + +import edu.wpi.first.wpilibj.command.Command; + +public class ElevatorMoveToSetpoint extends Command { + private boolean done; + public ElevatorMoveToSetpoint() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.elevator); + + } + public ElevatorMoveToSetpoint(int setpoint){ + requires(Robot.elevator); + Robot.elevator.setpoint = setpoint; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.elevator.elevatorMaster.set(ControlMode.MotionMagic,Robot.elevator.setpoint); + done = false; + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + if(Math.abs(Robot.elevator.elevatorMaster.getSelectedSensorPosition(0) -Robot.elevator.setpoint) < Constants.ELEVATOR_POSITION_BUFFER){ + done = true; + Robot.elevator.state = States.AT_SETPOINT; + + } + //Robot.elevator.updateSetpoint(); + + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return done; + } + + // 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/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/ElevatorPageDown.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/ElevatorPageDown.java new file mode 100644 index 0000000..59c6d9d --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/ElevatorPageDown.java @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.Elevator; + +import org.usfirst.frc100.Team100Robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class ElevatorPageDown extends Command { + public ElevatorPageDown() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.elevator); + + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + if(Robot.elevator.setpointLevel > 0){ + Robot.elevator.setpointLevel -= 1; + + } + Robot.elevator.setpoint = Robot.elevator.setpointsArray[Robot.elevator.setpointLevel].setpoint; + Robot.elevator.updateSetpoint(); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + @Override + protected void end() { + new ElevatorMoveToSetpoint().start(); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/ElevatorPageUp.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/ElevatorPageUp.java new file mode 100644 index 0000000..91996eb --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/ElevatorPageUp.java @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.Elevator; + +import org.usfirst.frc100.Team100Robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class ElevatorPageUp extends Command { + public ElevatorPageUp() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.elevator); + + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + if(Robot.elevator.setpointLevel < Robot.elevator.setpointsArray.length){ + Robot.elevator.setpointLevel += 1; + + } + Robot.elevator.setpoint = Robot.elevator.setpointsArray[Robot.elevator.setpointLevel].setpoint; + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + @Override + protected void end() { + new ElevatorMoveToSetpoint().start(); + + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/ElevatorTeleop.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/ElevatorTeleop.java new file mode 100644 index 0000000..ed16d2e --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/ElevatorTeleop.java @@ -0,0 +1,79 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.Elevator; + +import com.ctre.phoenix.motorcontrol.ControlMode; + +import org.usfirst.frc100.Team100Robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class ElevatorTeleop extends Command { + public ElevatorTeleop() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.elevator); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + System.out.println("INITIALIZED TELEOP ELEVATOR"); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + /* + if(Robot.oi.getManipulatorControl().getRawAxis(3) > 0 && (!Robot.elevator.upperLimitSwitch.get() && !Robot.elevator.intermediateLimitSwitch.get()) ){ //Going up and elev not at top + Robot.elevator.elevatorMaster.set(ControlMode.PercentOutput,-Robot.oi.getManipulatorControl().getRawAxis(3)); + } + else if(Robot.oi.getManipulatorControl().getRawAxis(3) < 0 && (!Robot.elevator.lowerLimitSwitch.get() && !Robot.elevator.intermediateDownLimitSwitch.get()) ){ //down up and elev not at bottom + Robot.elevator.elevatorMaster.set(ControlMode.PercentOutput,-Robot.oi.getManipulatorControl().getRawAxis(3)); + } + else{ + }*/ + SmartDashboard.putNumber("ELEV OI AXIS",Robot.oi.getManipulatorControl().getRawAxis(3)); + Robot.elevator.elevatorMaster.set(ControlMode.PercentOutput,-Robot.oi.getManipulatorControl().getRawAxis(3)); +/* + if(Robot.oi.getManipulatorControl().getRawAxis(3) <= 0 && !Robot.elevator.atMaxHeight){ //Going up + Robot.elevator.elevatorMaster.set(ControlMode.PercentOutput,-Robot.oi.getManipulatorControl().getRawAxis(3)*.4); + + }else if(Robot.oi.getManipulatorControl().getRawAxis(3) >= 0 && !Robot.elevator.atMinHeight){ //Going down + Robot.elevator.elevatorMaster.set(ControlMode.PercentOutput,-Robot.oi.getManipulatorControl().getRawAxis(3)*.3); + + } + else{ + System.out.println("Reached LS"); + + Robot.elevator.elevatorMaster.set(ControlMode.PercentOutput,0); + }*/ + + } + + // 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() { + System.out.println("ELEVATOR TELEOP ENDED"); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + System.out.println("ELEVATOR TELEOP INTERRUPTED"); + } +} diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/Homing/ElevatorHomingComplete.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/Homing/ElevatorHomingComplete.java new file mode 100644 index 0000000..858b57c --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/Homing/ElevatorHomingComplete.java @@ -0,0 +1,59 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.Elevator.Homing; + +import org.usfirst.frc100.Team100Robot.Robot; +import org.usfirst.frc100.Team100Robot.subsystems.Elevator.homingStates; +import org.usfirst.frc100.Team100Robot.subsystems.Elevator.States; + +import edu.wpi.first.wpilibj.command.Command; + +public class ElevatorHomingComplete extends Command { + public ElevatorHomingComplete() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.elevator); + + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.elevator.hs = homingStates.COMPLETE; + //TODO Change base robot state + } + + // 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 true; + } + + // Called once after isFinished returns true + @Override + protected void end() { + Robot.elevator.homed = true; + + Robot.elevator.state= States.MOVE_TO_SETPOINT; + System.out.println("HOMING COMPLETE"); + Robot.elevator.updateSetpoint(4000); + + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } +} diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/Homing/ElevatorHomingDown.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/Homing/ElevatorHomingDown.java new file mode 100644 index 0000000..587556e --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/Homing/ElevatorHomingDown.java @@ -0,0 +1,60 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.Elevator.Homing; + + +import com.ctre.phoenix.motorcontrol.ControlMode; + +import org.usfirst.frc100.Team100Robot.Robot; +import org.usfirst.frc100.Team100Robot.subsystems.Elevator.homingStates; + +import edu.wpi.first.wpilibj.command.Command; + +public class ElevatorHomingDown extends Command { + public ElevatorHomingDown() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.elevator); + + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.elevator.hs = homingStates.ELEV_AT_LIMIT_SWITCH; + Robot.elevator.elevatorMaster.set(ControlMode.PercentOutput,0); + System.out.println("HOMING DOWN START"); + + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + System.out.println("In homing down"); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + @Override + protected void end() { + System.out.println("Homing Down Done"); + new ElevatorHomingGoingUp().start(); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } +} diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/Homing/ElevatorHomingFatalError.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/Homing/ElevatorHomingFatalError.java new file mode 100644 index 0000000..710c012 --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/Homing/ElevatorHomingFatalError.java @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.Elevator.Homing; + +import org.usfirst.frc100.Team100Robot.Robot; +import org.usfirst.frc100.Team100Robot.subsystems.Elevator.homingStates; + +import edu.wpi.first.wpilibj.command.Command; + +public class ElevatorHomingFatalError extends Command { + public ElevatorHomingFatalError() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.elevator); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.elevator.hs = homingStates.FATAL; + } + + // 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() { + end(); + } +} diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/Homing/ElevatorHomingGoingDown.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/Homing/ElevatorHomingGoingDown.java new file mode 100644 index 0000000..6957fb8 --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/Homing/ElevatorHomingGoingDown.java @@ -0,0 +1,67 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.Elevator.Homing; + + +import com.ctre.phoenix.motorcontrol.ControlMode; + +import org.usfirst.frc100.Team100Robot.Constants; +import org.usfirst.frc100.Team100Robot.Robot; +import org.usfirst.frc100.Team100Robot.subsystems.Elevator.homingStates; + +import edu.wpi.first.wpilibj.command.Command; + +public class ElevatorHomingGoingDown extends Command { + private boolean complete = false; + public ElevatorHomingGoingDown() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.elevator); + complete = false; + + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.elevator.hs = homingStates.ELEV_GOING_DOWN; + Robot.elevator.elevatorMaster.set(ControlMode.PercentOutput, Constants.HOMING_GOING_DOWN_POWER); + complete = false; + System.out.println("In Init"); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + if(Robot.elevator.atMinHeight){ + complete = true; + System.out.println("atMinHeight"); + } + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return complete; + } + + // Called once after isFinished returns true + @Override + protected void end() { + System.out.println("END"); + new ElevatorHomingDown().start(); + + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } +} diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/Homing/ElevatorHomingGoingUp.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/Homing/ElevatorHomingGoingUp.java new file mode 100644 index 0000000..35245f4 --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/Homing/ElevatorHomingGoingUp.java @@ -0,0 +1,63 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.Elevator.Homing; + +import com.ctre.phoenix.motorcontrol.ControlMode; + +import org.usfirst.frc100.Team100Robot.Constants; +import org.usfirst.frc100.Team100Robot.Robot; +import org.usfirst.frc100.Team100Robot.subsystems.Elevator.homingStates; + +import edu.wpi.first.wpilibj.command.Command; + +public class ElevatorHomingGoingUp extends Command { + private boolean complete = false; + public ElevatorHomingGoingUp() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.elevator); + complete = false; + + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.elevator.hs = homingStates.ELEV_RISING; + complete = false; + Robot.elevator.elevatorMaster.set(ControlMode.PercentOutput,Constants.HOMING_GOING_UP_POWER); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + if(!Robot.elevator.atMinHeight){ + complete = true; + } + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return complete; + } + + // Called once after isFinished returns true + @Override + protected void end() { + Robot.elevator.elevatorMaster.setSelectedSensorPosition(0); + new ElevatorHomingComplete().start(); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } +} diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/Homing/ElevatorHomingInit.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/Homing/ElevatorHomingInit.java new file mode 100644 index 0000000..1da0df4 --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Elevator/Homing/ElevatorHomingInit.java @@ -0,0 +1,63 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.Elevator.Homing; + +import org.usfirst.frc100.Team100Robot.Robot; +import org.usfirst.frc100.Team100Robot.subsystems.Elevator.homingStates; + +import edu.wpi.first.wpilibj.command.Command; + +public class ElevatorHomingInit extends Command { + public ElevatorHomingInit() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.elevator); + + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.elevator.hs = homingStates.INIT; + System.out.println("INIT STARTED"); + } + + // 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 true; + } + + // Called once after isFinished returns true + @Override + protected void end() { + System.out.println("INIT FINISHED"); + if(Robot.elevator.atMinHeight){ + new ElevatorHomingDown().start(); + + } + else{ + new ElevatorHomingGoingDown().start(); + } + + + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } +} diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/HatchManipulator/Bill/BillLower.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/HatchManipulator/Bill/BillLower.java new file mode 100644 index 0000000..eea50d8 --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/HatchManipulator/Bill/BillLower.java @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.HatchManipulator.Bill; + +import org.usfirst.frc100.Team100Robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class BillLower extends Command { + public BillLower() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.manipulator); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.manipulator.bill.set(true); + } + + // 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() { + Robot.manipulator.bill.set(false); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } +} diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/HatchManipulator/Bill/BillRaise.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/HatchManipulator/Bill/BillRaise.java new file mode 100644 index 0000000..4482695 --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/HatchManipulator/Bill/BillRaise.java @@ -0,0 +1,47 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.HatchManipulator.Bill; + +import org.usfirst.frc100.Team100Robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class BillRaise extends Command { + public BillRaise() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.manipulator); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // 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/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/HatchManipulator/Pusher/ExtendPusher.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/HatchManipulator/Pusher/ExtendPusher.java new file mode 100644 index 0000000..3f6c03f --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/HatchManipulator/Pusher/ExtendPusher.java @@ -0,0 +1,49 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.HatchManipulator.Pusher; + +import org.usfirst.frc100.Team100Robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class ExtendPusher extends Command { + public ExtendPusher() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.manipulator); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.manipulator.hatchPusher.set(true); + } + + // 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() { + end(); + } +} diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/HatchManipulator/Pusher/RetractPusher.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/HatchManipulator/Pusher/RetractPusher.java new file mode 100644 index 0000000..a5265f2 --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/HatchManipulator/Pusher/RetractPusher.java @@ -0,0 +1,49 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.HatchManipulator.Pusher; + +import org.usfirst.frc100.Team100Robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class RetractPusher extends Command { + public RetractPusher() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.manipulator); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.manipulator.hatchPusher.set(false); + } + + // 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() { + end(); + } +} diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/HighGear.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/HighGear.java index ed0ccc1..d2cdec9 100644 --- a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/HighGear.java +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/HighGear.java @@ -30,7 +30,7 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - Robot.shifter.getShifter().set(true);; + //Robot.shifter.getShifter().set(true);; } // Make this return true when this Command no longer needs to run execute() diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/IntakeArm/IntakeArmIntakeElement.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/IntakeArm/IntakeArmIntakeElement.java new file mode 100644 index 0000000..52f7329 --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/IntakeArm/IntakeArmIntakeElement.java @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.IntakeArm; + +import org.usfirst.frc100.Team100Robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class IntakeArmIntakeElement extends Command { + private boolean done = false; + public IntakeArmIntakeElement() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.cargoPickup); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + System.out.println("INTAKE INIT"); + Robot.cargoPickup.setOutput(-0.3); + + done = false; + } + + // 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() { + Robot.cargoPickup.setOutput(0); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } +} diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/IntakeArm/IntakeArmPivot/IntakeArmDown.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/IntakeArm/IntakeArmPivot/IntakeArmDown.java new file mode 100644 index 0000000..c536a7b --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/IntakeArm/IntakeArmPivot/IntakeArmDown.java @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.IntakeArm.IntakeArmPivot; + +import org.usfirst.frc100.Team100Robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class IntakeArmDown extends Command { + public IntakeArmDown() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.cargoPickup); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.cargoPickup.cargoIntakeArmPivot.set(false); + + } + + // 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() { + end(); + } +} diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/IntakeArm/IntakeArmPivot/IntakeArmUp.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/IntakeArm/IntakeArmPivot/IntakeArmUp.java new file mode 100644 index 0000000..1baa502 --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/IntakeArm/IntakeArmPivot/IntakeArmUp.java @@ -0,0 +1,51 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.IntakeArm.IntakeArmPivot; + +import org.usfirst.frc100.Team100Robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class IntakeArmUp extends Command { + public IntakeArmUp() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.cargoPickup); + + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.cargoPickup.cargoIntakeArmPivot.set(true); + + } + + // 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() { + end(); + } +} diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/LowGear.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/LowGear.java index 5e61963..c633f6e 100644 --- a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/LowGear.java +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/LowGear.java @@ -45,7 +45,7 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - Robot.shifter.getShifter().set(false); + //Robot.shifter.getShifter().set(false); } // Make this return true when this Command no longer needs to run execute() diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Shoulder/ShoulderDefault.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Shoulder/ShoulderDefault.java new file mode 100644 index 0000000..28cd2a8 --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Shoulder/ShoulderDefault.java @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.Shoulder; + +import com.ctre.phoenix.motorcontrol.ControlMode; + +import org.usfirst.frc100.Team100Robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class ShoulderDefault extends Command { + public ShoulderDefault() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.carriageShoulder); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.carriageShoulder.carriageShoulderMotor.set(ControlMode.PercentOutput,0); + } + + // 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/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Shoulder/ShoulderDown.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Shoulder/ShoulderDown.java new file mode 100644 index 0000000..8c39a2c --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Shoulder/ShoulderDown.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.Shoulder; + +import org.usfirst.frc100.Team100Robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class ShoulderDown extends Command { + public ShoulderDown() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.carriageShoulder); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.carriageShoulder.moveDown(); + } + + // 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 true; + } + + // 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/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Shoulder/ShoulderTeleop.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Shoulder/ShoulderTeleop.java new file mode 100644 index 0000000..bf4efb6 --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Shoulder/ShoulderTeleop.java @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.Shoulder; + +import com.ctre.phoenix.motorcontrol.ControlMode; + +import org.usfirst.frc100.Team100Robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class ShoulderTeleop extends Command { + public ShoulderTeleop() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.carriageShoulder); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + SmartDashboard.putNumber("OI Val",Robot.oi.getManipulatorControl().getRawAxis(1)); + Robot.carriageShoulder.carriageShoulderMotor.set(ControlMode.PercentOutput,Robot.oi.getManipulatorControl().getRawAxis(1)); + } + + // 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/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Shoulder/ShoulderUp.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Shoulder/ShoulderUp.java new file mode 100644 index 0000000..2dfec62 --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/commands/Shoulder/ShoulderUp.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.commands.Shoulder; + +import org.usfirst.frc100.Team100Robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class ShoulderUp extends Command { + public ShoulderUp() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.carriageShoulder); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.carriageShoulder.moveUp(); + } + + // 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 true; + } + + // 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/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/CargoHatchScore.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/CargoHatchScore.java index 10bc92f..8dc89d0 100644 --- a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/CargoHatchScore.java +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/CargoHatchScore.java @@ -9,6 +9,8 @@ // it from being updated in the future. +//TODO Delete this file + package org.usfirst.frc100.Team100Robot.subsystems; import edu.wpi.first.wpilibj.command.Subsystem; @@ -32,15 +34,19 @@ public class CargoHatchScore extends Subsystem { public CargoHatchScore() { topBallRollerShooter = new WPI_TalonSRX(Constants.CARGO_HATCH_SCORER_TOP_CANID); bottomBallRollerShooter = new WPI_TalonSRX(Constants.CARGO_HATCH_SCORER_BOTTOM_CANID); + topBallRollerShooter.overrideLimitSwitchesEnable(false); + topBallRollerShooter.configPeakOutputForward(0.25); + topBallRollerShooter.configPeakOutputReverse(-0.25); + bottomBallRollerShooter.configPeakOutputForward(0.25); + bottomBallRollerShooter.configPeakOutputReverse(-0.25); + //loadingStationIntake = new Solenoid(Constants.PCM_CANID, Constants.LOADING_STATION_INTAKE_PCMID); + //addChild("LoadingStationIntake", loadingStationIntake); - loadingStationIntake = new Solenoid(Constants.PCM_CANID, Constants.LOADING_STATION_INTAKE_PCMID); - addChild("LoadingStationIntake", loadingStationIntake); - - hatchScorer = new Solenoid(Constants.PCM_CANID, Constants.HATCH_SCORER_PCMID); - addChild("HatchScorer", hatchScorer); + // hatchScorer = new Solenoid(Constants.PCM_CANID, Constants.HATCH_SCORER_PCMID); + // addChild("HatchScorer", hatchScorer); - cargoScorer = new Solenoid(Constants.PCM_CANID, Constants.CARGO_SCORER_PCMID); - addChild("CargoScorer", cargoScorer); + //cargoScorer = new Solenoid(Constants.PCM_CANID, Constants.CARGO_SCORER_PCMID); + // addChild("CargoScorer", cargoScorer); } @Override diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/CargoPickup.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/CargoPickup.java index cd121f2..78951ce 100644 --- a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/CargoPickup.java +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/CargoPickup.java @@ -12,6 +12,8 @@ package org.usfirst.frc100.Team100Robot.subsystems; import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import org.usfirst.frc100.Team100Robot.Constants; @@ -20,33 +22,49 @@ import edu.wpi.first.wpilibj.VictorSP; /** - * + * */ public class CargoPickup extends Subsystem { - private WPI_TalonSRX tiltCargoFloorPickup; - private VictorSP rollerCargoFloorPickup; - private Solenoid cargoGroundPickup; + //public WPI_TalonSRX cargoTilt; + public VictorSP cargoRoller1; + public VictorSP cargoRoller2; + public Solenoid cargoIntakeArmPivot; + public Solenoid cargoIntakeArmPivot2; public CargoPickup() { - tiltCargoFloorPickup = new WPI_TalonSRX(Constants.CARGO_PICKUP_TILT_PWM); + //cargoTilt = new WPI_TalonSRX(Constants.CARGO_PICKUP_TILT_CANID); + cargoIntakeArmPivot = new Solenoid(Constants.PCM_CANID,Constants.CARGO_GROUND_PICKUP_PCMID); + cargoIntakeArmPivot2 = new Solenoid(Constants.PCM_CANID,Constants.CARGO_GROUND_PICKUP2_PCMID); + cargoRoller1 = new VictorSP(Constants.CARGO_PICKUP_ROLLER1_PWM); + addChild("CargoRoller1", cargoRoller1); + cargoRoller1.setInverted(false); + + cargoRoller2 = new VictorSP(Constants.CARGO_PICKUP_ROLLER2_PWM); + addChild("CargoRoller2", cargoRoller2); + cargoRoller2.setInverted(false); - rollerCargoFloorPickup = new VictorSP(Constants.CARGO_PICKUP_ROLLER_PWM); - addChild("RollerCargoFloorPickup", rollerCargoFloorPickup); - rollerCargoFloorPickup.setInverted(false); - cargoGroundPickup = new Solenoid(Constants.PCM_CANID, Constants.CARGO_GROUND_PICKUP_PCMID); - addChild("CargoGroundPickup", cargoGroundPickup); } @Override public void initDefaultCommand() { // setDefaultCommand(new MySpecialCommand()); + // setDefaultCommand(new CargoManipulator()); } @Override public void periodic() { // Put code here to be run every loop + SmartDashboard.putNumber("6 PO",cargoRoller1.get()); + SmartDashboard.putData("CargoIntakeArmPivot", cargoIntakeArmPivot); + SmartDashboard.putData("CargoIntakeArmPivot2",cargoIntakeArmPivot2); + } + + public void setOutput(double output){ + System.out.println("SETTING TO "+output); + cargoRoller1.set(output); + cargoRoller2.set(output); } // Put methods for controlling this subsystem // here. Call these from Commands. diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/CarriageShoulder.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/CarriageShoulder.java index 975cc57..6a924bf 100644 --- a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/CarriageShoulder.java +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/CarriageShoulder.java @@ -12,33 +12,92 @@ package org.usfirst.frc100.Team100Robot.subsystems; import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import org.usfirst.frc100.Team100Robot.Constants; +import org.usfirst.frc100.Team100Robot.commands.Shoulder.ShoulderDefault; +import org.usfirst.frc100.Team100Robot.commands.Shoulder.ShoulderTeleop; /** * */ public class CarriageShoulder extends Subsystem { - private WPI_TalonSRX carriageShoulderMotor; + public int currentSetpointIndex = 0; + public int currentSetpoint = -1; + public static final int[] setpoints = {100,200,300}; + + public WPI_TalonSRX carriageShoulderMotor; public CarriageShoulder() { carriageShoulderMotor = new WPI_TalonSRX(Constants.ELEVATOR_CARRIAGE_SHOULDER_CANID); + carriageShoulderMotor.configFactoryDefault(); + carriageShoulderMotor.setSelectedSensorPosition(0); + carriageShoulderMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, Constants.SHOULDER_MASTER_TIMEOUT); + carriageShoulderMotor.setInverted(true); + carriageShoulderMotor.setSensorPhase(true); + carriageShoulderMotor.configPeakOutputForward(0.25); + carriageShoulderMotor.configPeakOutputReverse(-0.25); + carriageShoulderMotor.configNominalOutputForward(0); + carriageShoulderMotor.configNominalOutputReverse(0); + carriageShoulderMotor.configAllowableClosedloopError(0, 10, Constants.SHOULDER_MASTER_TIMEOUT); + carriageShoulderMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0,10,Constants.SHOULDER_MASTER_TIMEOUT); + carriageShoulderMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10,Constants.SHOULDER_MASTER_TIMEOUT); + carriageShoulderMotor.configMotionCruiseVelocity(15000,Constants.SHOULDER_MASTER_TIMEOUT); + carriageShoulderMotor.configMotionAcceleration(6000,Constants.SHOULDER_MASTER_TIMEOUT); + + carriageShoulderMotor.enableCurrentLimit(false); + carriageShoulderMotor.overrideLimitSwitchesEnable(false); + carriageShoulderMotor.config_kP(0, Constants.ELEVATOR_KP); + carriageShoulderMotor.config_kI(0, Constants.ELEVATOR_KI); + carriageShoulderMotor.config_kD(0, Constants.ELEVATOR_KD); + carriageShoulderMotor.config_kF(0, Constants.ELEVATOR_KF); } @Override public void initDefaultCommand() { // Set the default command for a subsystem here. // setDefaultCommand(new MySpecialCommand()); + /*currentSetpointIndex = 0; + this.updateSetpointGivenIndex();*/ + //setDefaultCommand(new ShoulderDefault()); + + setDefaultCommand(new ShoulderTeleop()); } @Override public void periodic() { // Put code here to be run every loop - + SmartDashboard.putNumber("Shoulder Enc",this.carriageShoulderMotor.getSelectedSensorPosition()); + SmartDashboard.putNumber("Shoulder PO", this.carriageShoulderMotor.getMotorOutputPercent()); + SmartDashboard.putString("Shoulder CM", this.carriageShoulderMotor.getControlMode().toString()); + SmartDashboard.putNumber("Shoulder Setpoint",this.currentSetpoint); + if(this.carriageShoulderMotor.getControlMode() == ControlMode.MotionMagic){ + SmartDashboard.putNumber("Shoulder Error", this.carriageShoulderMotor.getClosedLoopError()); + } } // Put methods for controlling this subsystem // here. Call these from Commands. + public void updateSetpointGivenIndex(){ + this.currentSetpoint = this.setpoints[this.currentSetpointIndex]; + //this.carriageShoulderMotor.set(ControlMode.MotionMagic,this.currentSetpoint); + } + public void moveUp(){ + if(this.currentSetpoint < this.setpoints.length - 1){ + this.currentSetpoint += 1; + } + this.updateSetpointGivenIndex(); + } + public void moveDown(){ + if(this.currentSetpoint > 0){ + this.currentSetpoint -= 1; + } + this.updateSetpointGivenIndex(); + } } diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/Climber.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/Climber.java index 6a8bca0..eb8f345 100644 --- a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/Climber.java +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/Climber.java @@ -12,6 +12,8 @@ package org.usfirst.frc100.Team100Robot.subsystems; import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; @@ -30,7 +32,8 @@ public class Climber extends Subsystem { public Climber() { climberMaster = new WPI_TalonSRX(Constants.CLIMBER_MASTER_CANID); - + climberMaster.configPeakOutputForward(0.25); + climberMaster.configPeakOutputReverse(-0.25); deploy = new Solenoid(Constants.PCM_CANID, Constants.CLIMBER_DEPLOY_PCMID); addChild("Deploy",deploy); @@ -48,6 +51,7 @@ public void initDefaultCommand() { @Override public void periodic() { // Put code here to be run every loop + SmartDashboard.putData("ClimberDeploySolenoid",deploy); } // Put methods for controlling this subsystem diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/Drivetrain.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/Drivetrain.java index 7820751..44e283d 100644 --- a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/Drivetrain.java +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/Drivetrain.java @@ -17,10 +17,11 @@ import org.usfirst.frc100.Team100Robot.Constants; import org.usfirst.frc100.Team100Robot.Robot; -import org.usfirst.frc100.Team100Robot.commands.Drive; +import org.usfirst.frc100.Team100Robot.commands.Drivetrain.Drive; import edu.wpi.first.wpilibj.PIDController; import edu.wpi.first.wpilibj.PIDOutput; +import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -35,8 +36,10 @@ public class Drivetrain extends Subsystem implements PIDOutput { private WPI_VictorSPX leftFollower; private WPI_VictorSPX rightFollower; public PIDController turnPID; + public Solenoid shift; public Drivetrain() { + shift = new Solenoid(Constants.PCM_CANID,Constants.DRIVETRAIN_SHIFTER_PCMID); leftMaster = new WPI_TalonSRX(Constants.DRIVE_TRAIN_LEFT_MASTER_CANID); rightMaster = new WPI_TalonSRX(Constants.DRIVE_TRAIN_RIGHT_MASTER_CANID); @@ -56,12 +59,16 @@ public Drivetrain() { leftFollower.setInverted(Constants.DRIVE_TRAIN_LEFT_FOLLOWER_INVERT); rightMaster.setInverted(Constants.DRIVE_TRAIN_RIGHT_MASTER_INVERT); rightFollower.setInverted(Constants.DRIVE_TRAIN_RIGHT_FOLLOWER_INVERT); + leftMaster.overrideLimitSwitchesEnable(false); + rightMaster.overrideLimitSwitchesEnable(false); + - turnPID = new PIDController(Constants.DT_TURN_P, Constants.DT_TURN_I, Constants.DT_TURN_D, Robot.ahrs, this); + /*turnPID = new PIDController(Constants.DT_TURN_P, Constants.DT_TURN_I, Constants.DT_TURN_D, Robot.ahrs, this); turnPID.setInputRange(Constants.DT_TURN_MIN_ROTATION_ANGLE, Constants.DT_TURN_MAX_ROTATION_ANGLE); turnPID.setContinuous(true); turnPID.setOutputRange(Constants.DT_TURN_MIN_OUTPUT, Constants.DT_TURN_MAX_OUTPUT); turnPID.setAbsoluteTolerance(Constants.DT_TURN_ABSOLUTE_TOLERANCE); + */ } public void turn(double leftPower, double rightPower){ @@ -70,7 +77,7 @@ public void turn(double leftPower, double rightPower){ } public void pidTurn(){ - turn(turnPID.get(), turnPID.get()); + //turn(turnPID.get(), turnPID.get()); } @Override @@ -91,10 +98,16 @@ public void periodic() { // Put code here to be run every loop SmartDashboard.putNumber("ENC LEFT",leftMaster.getSelectedSensorPosition()); SmartDashboard.putNumber("ENC RIGHT", rightMaster.getSelectedSensorPosition()); + SmartDashboard.putNumber("PO LEFT",leftMaster.getMotorOutputPercent()); + SmartDashboard.putNumber("PO RIGHT",rightMaster.getMotorOutputPercent()); + SmartDashboard.putBoolean("SHIFT State", shift.get()); + SmartDashboard.putNumber("SHIFT ID", Constants.DRIVETRAIN_SHIFTER_PCMID); + SmartDashboard.putData("SHIFT PCM", shift); } public void drive(){ differentialDrive.arcadeDrive(-Robot.oi.getLeftStick().getY(), Robot.oi.getRightStick().getX()); + } @Override diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/Elevator.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/Elevator.java index 5f0d353..3e79593 100644 --- a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/Elevator.java +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/Elevator.java @@ -12,39 +12,287 @@ package org.usfirst.frc100.Team100Robot.subsystems; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Preferences; import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; import org.usfirst.frc100.Team100Robot.Constants; +import org.usfirst.frc100.Team100Robot.commands.Elevator.ElevatorAtSetpoint; +import org.usfirst.frc100.Team100Robot.commands.Elevator.ElevatorMoveToSetpoint; +import org.usfirst.frc100.Team100Robot.commands.Elevator.ElevatorTeleop; +import org.usfirst.frc100.Team100Robot.commands.Elevator.Homing.ElevatorHomingInit; + -/** - * - */ public class Elevator extends Subsystem { - private WPI_TalonSRX elevatorMaster; - private WPI_VictorSPX elevatorFollower; + + public WPI_TalonSRX elevatorMaster; + public WPI_VictorSPX elevatorFollower; + public DigitalInput carriageLowerLimitSwitch = new DigitalInput(0); + public DigitalInput carriageUpperLimitSwitch = new DigitalInput(1); + public DigitalInput intermediateUpperLimitSwitch = new DigitalInput(9); + public DigitalInput intermediateLowerLimitSwitch = new DigitalInput(3); + public int setpoint; + + public int setpointLevel = 0; + /** + * Enum for possible homing states of the elevator + */ + public enum homingStates{ + INIT,ELEV_GOING_DOWN,ELEV_AT_LIMIT_SWITCH,ELEV_RISING,COMPLETE,FATAL + } + /** + * The homing state of the elevator + */ + public homingStates hs; + /** + * Possible states for the elevator to be in + */ + public enum States{ + AT_SETPOINT,MOVE_TO_SETPOINT,HOMING,TELEOP + } + /** + * The current state of the elevator + */ + public States state; + + public boolean atMaxHeight = false; + public boolean atMinHeight = false; + + /** + * Whether Preferences or Constants should be used for PID values + *
+ * false uses Constants.java for PID values + * true uses NT Preferences for PID values with Constants.java as the fallback + */ + public static final boolean ELEVATOR_USE_PREFERENCES_FOR_PID_VALUES = false; + + /** + * Instance of Robot Preferences + */ + public Preferences prefs; + + /** + * Post Constants PID values to preferences + *
+ * Run this to initialize preferences or to make preferences up to date with the constants + */ + public static final boolean ELEVATOR_POST_PID_CONSTANTS_TO_NT_PREFERENCES = true; + + /** + * true Disables intelligent control (PID, Setpoints, homing) + *
+ * false Enables intelligent control (PID, Setpoints, homing) [SHOULD ALMOST ALWAYS BE SET TO] + *
+ * This should ONLY be used for elevator testing and SHOULD NEVER BE ON DURING COMPETITION + */ + public static final boolean DISABLE_INTELLIGENT_CONTROL = true; + + public boolean homed = false; + + public Elevator() { + prefs = Preferences.getInstance(); + elevatorMaster = new WPI_TalonSRX(Constants.ELEVATOR_MASTER_CANID); elevatorFollower = new WPI_VictorSPX(Constants.ELEVATOR_FOLLOWER_CANID); + elevatorMaster.configFactoryDefault(); + elevatorFollower.configFactoryDefault(); + elevatorMaster.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, Constants.ELEVATOR_MASTER_TIMEOUT); + elevatorMaster.setInverted(true); + elevatorFollower.setInverted(true); + elevatorMaster.setSensorPhase(true); + elevatorMaster.configPeakOutputForward(0.25); + elevatorMaster.configPeakOutputReverse(-0.25); + elevatorMaster.configNominalOutputForward(0); + elevatorMaster.configNominalOutputReverse(0); + elevatorMaster.configAllowableClosedloopError(0, 0, Constants.ELEVATOR_MASTER_TIMEOUT); + elevatorMaster.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0,10,Constants.ELEVATOR_MASTER_TIMEOUT); + elevatorMaster.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10,Constants.ELEVATOR_MASTER_TIMEOUT); + elevatorMaster.configMotionCruiseVelocity(15000,Constants.ELEVATOR_MASTER_TIMEOUT); + elevatorMaster.configMotionAcceleration(6000,Constants.ELEVATOR_MASTER_TIMEOUT); + elevatorMaster.enableCurrentLimit(false); + elevatorMaster.overrideLimitSwitchesEnable(false); + elevatorFollower.follow(elevatorMaster); + + + if(ELEVATOR_POST_PID_CONSTANTS_TO_NT_PREFERENCES){ + prefs.putDouble("ELEVATOR_KP", Constants.ELEVATOR_KP); + prefs.putDouble("ELEVATOR_KI", Constants.ELEVATOR_KI); + prefs.putDouble("ELEVATOR_KD", Constants.ELEVATOR_KD); + prefs.putDouble("ELEVATOR_KF", Constants.ELEVATOR_KF); + } + if(ELEVATOR_USE_PREFERENCES_FOR_PID_VALUES){ + elevatorMaster.config_kP(0, prefs.getDouble("ELEVATOR_KP",Constants.ELEVATOR_KP)); + elevatorMaster.config_kI(0, prefs.getDouble("ELEVATOR_KI",Constants.ELEVATOR_KI)); + elevatorMaster.config_kD(0, prefs.getDouble("ELEVATOR_KD",Constants.ELEVATOR_KD)); + elevatorMaster.config_kF(0, prefs.getDouble("ELEVATOR_KF",Constants.ELEVATOR_KF)); + + } + else{ + elevatorMaster.config_kP(0, Constants.ELEVATOR_KP); + elevatorMaster.config_kI(0, Constants.ELEVATOR_KI); + elevatorMaster.config_kD(0, Constants.ELEVATOR_KD); + elevatorMaster.config_kF(0, Constants.ELEVATOR_KF); + } + + + //elevatorMaster.configClosedloopRamp(0.4); + //elevatorMaster.configPeakCurrentLimit(20); + //elevatorMaster.configPeakCurrentLimit(60); + homed = false; + + } + + /** + * Template for a setpoint + */ + public class Setpoint{ + /** + * Name or any notes regarding to the Setpoint + */ + public String annotation; + /** + * The setpoint value + */ + public int setpoint = -1; + /** + * The location within the Setpoint array + */ + public int arrayIndex = -1; + /** + * Creates a new setpoint + * @param annotation Name or any other useful information + * @param setpoint The value of the setpoint + * @param arrayIndex The index of the setpoint in the setpointsArray + */ + public Setpoint(String annotation, double setpoint, int arrayIndex){ + this.setpoint = convertInchesToTicks(setpoint); + this.annotation = annotation; + this.arrayIndex = arrayIndex; + + } + } + + public Setpoint[] setpointsArray = {new Setpoint("", 19, 0),new Setpoint("",27.5,1),new Setpoint("",47,2),new Setpoint("",55.5,3),new Setpoint("ROCKET_HIGH",75,4),new Setpoint("",77.5,5)}; + + /** + * Set the setpoint for the Talon SRX given the setpoint instance variable + */ + public void updateSetpoint(){ + System.out.println("FINAL SP"+this.setpoint); + new ElevatorMoveToSetpoint().start(); + } + /** + * Set the setpoint for the Talon SRX + * @param setpoint + */ + public void updateSetpoint(int setpoint){ + this.setpoint = setpoint; + System.out.println("SETPOINT: "+this.setpoint); + this.updateSetpoint(); + } @Override public void initDefaultCommand() { // Set the default command for a subsystem here. // setDefaultCommand(new MySpecialCommand()); + if(DISABLE_INTELLIGENT_CONTROL){ + setDefaultCommand(new ElevatorTeleop()); + }else{ + setDefaultCommand(new ElevatorAtSetpoint()); + //new ElevatorHomingInit().start(); + //setDefaultCommand(new ElevatorHomingInit()); + } + System.out.println(this.getDefaultCommandName()); + } @Override public void periodic() { // Put code here to be run every loop + //updateSD(); + SmartDashboard.putString("ELEV COMMAND", this.getCurrentCommandName()); + SmartDashboard.putBoolean("Carriage Lower Limit Switch",this.carriageLowerLimitSwitch.get()); + SmartDashboard.putBoolean("Carriage Upper Limit Switch",this.carriageUpperLimitSwitch.get()); + SmartDashboard.putBoolean("Intermediate Lower Limit Switch",this.intermediateLowerLimitSwitch.get()); + SmartDashboard.putBoolean("Intermediate Upper Limit Switch",this.intermediateUpperLimitSwitch.get()); + SmartDashboard.putNumber("ELEV ENC",this.elevatorMaster.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("ELEV PercentOutput", this.elevatorMaster.getMotorOutputPercent()); + SmartDashboard.putNumber("ELEV Setpoint",this.setpoint); + SmartDashboard.putString("ELEV ControlMode",this.elevatorMaster.getControlMode().toString()); + + SmartDashboard.putBoolean("AT TOP", this.atMaxHeight); + SmartDashboard.putBoolean("AT BOTTOM",this.atMinHeight); + + if(this.intermediateUpperLimitSwitch.get() + && this.carriageUpperLimitSwitch.get() + && !this.carriageLowerLimitSwitch.get() + && !this.intermediateLowerLimitSwitch.get()){ + this.atMinHeight = true; + }else{this.atMinHeight = false;} + + if(!this.intermediateUpperLimitSwitch.get() && + !this.carriageUpperLimitSwitch.get() && + this.carriageLowerLimitSwitch.get() && + this.intermediateLowerLimitSwitch.get()){ + this.atMaxHeight = true; + }else{this.atMaxHeight = false;} + /*if(this.atMinHeight && elevatorMaster.getMotorOutputPercent() < 0){ + elevatorMaster.set(ControlMode.PercentOutput,0); + } + else if(this.atMaxHeight && elevatorMaster.getMotorOutputPercent() > 0){ + elevatorMaster.set(ControlMode.PercentOutput,0); + }*/ } + /** + * Updates SmartDashboard + */ + public void updateSD(){ + /* + if(state == States.MOVE_TO_SETPOINT || state == States.AT_SETPOINT){ + SmartDashboard.putNumber("ELEV_location", elevatorMaster.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("ELEV_percentoutput",elevatorMaster.getMotorOutputPercent()); + SmartDashboard.putNumber("ELEV_setpoint",setpoint); + SmartDashboard.putNumber("ELEV_setpointLevel",setpointLevel); + SmartDashboard.putBoolean("ELEV_usingPreferencesForPIDValues", ELEVATOR_USE_PREFERENCES_FOR_PID_VALUES); + SmartDashboard.putNumber("ELEV_error",elevatorMaster.getClosedLoopError()); + SmartDashboard.putNumber("ELEV_talontarget",elevatorMaster.getClosedLoopTarget()); + SmartDashboard.putString("ELEV_state",state.toString()); + SmartDashboard.putNumber("ELEV_activeTrajectoryVelocity",elevatorMaster.getActiveTrajectoryVelocity()); + SmartDashboard.putNumber("ELEV_outputCurrent",elevatorMaster.getOutputCurrent()); + SmartDashboard.putNumber("ELEV_outputVoltage",elevatorMaster.getMotorOutputVoltage()); + //SmartDashboard.putString("ELEV/homeState",hs.toString()); + + } +*/ + } + // Put methods for controlling this subsystem // here. Call these from Commands. + /** + * + * @param inches + * @return the height in encoder ticks + */ + public static int convertInchesToTicks(double inches){ + if(inches > Constants.ELEVATOR_MAX_HEIGHT_IN_INCHES){ + System.out.println("INCHES EXCEED MAX"); + return (int)Constants.ELEVATOR_MAX_HEIGHT_IN_INCHES * Constants.ELEVATOR_INCH_TO_ENCODER_CONVERSION_FACTION; + } + return (int)((inches- Constants.ELEVATOR_START_HEIGHT_IN_INCHES) * Constants.ELEVATOR_INCH_TO_ENCODER_CONVERSION_FACTION ); + } } diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/HatchPickup.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/HatchPickup.java index 1796be9..ae2d494 100644 --- a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/HatchPickup.java +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/HatchPickup.java @@ -12,6 +12,8 @@ package org.usfirst.frc100.Team100Robot.subsystems; import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import org.usfirst.frc100.Team100Robot.Constants; @@ -47,6 +49,7 @@ public void initDefaultCommand() { @Override public void periodic() { // Put code here to be run every loop + SmartDashboard.putData(hatchFloorPickup); } diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/Manipulator.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/Manipulator.java new file mode 100644 index 0000000..40455ea --- /dev/null +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/Manipulator.java @@ -0,0 +1,62 @@ +/*----------------------------------------------------------------------------*/ +/* 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.frc100.Team100Robot.subsystems; + +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + +import org.usfirst.frc100.Team100Robot.Constants; +import org.usfirst.frc100.Team100Robot.commands.CargoManipulator.CargoManipulatorDefault; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Solenoid; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * Add your docs here. + */ +public class Manipulator extends Subsystem { + // Put methods for controlling this subsystem + // here. Call these from Commands. + public WPI_TalonSRX topRoller; + public WPI_TalonSRX bottomRoller; + public DigitalInput cargoSensor; + public Solenoid bill; + public Solenoid hatchPusher; + public Solenoid cargoScorer; + public Manipulator(){ + topRoller = new WPI_TalonSRX(Constants.CARGO_MANIPULATOR_TOP_TALONSRX_ID); + topRoller.overrideLimitSwitchesEnable(false); + topRoller.configPeakOutputForward(0.25); + topRoller.configPeakOutputReverse(-0.25); + bottomRoller = new WPI_TalonSRX(Constants.CARGO_MANIPULATOR_BOTTOM_TALONSRX_ID); + bottomRoller.overrideLimitSwitchesEnable(false); + bottomRoller.configPeakOutputForward(0.25); + bottomRoller.configPeakOutputReverse(-0.25); + bottomRoller.follow(topRoller); + cargoSensor = new DigitalInput(4); + bill = new Solenoid(Constants.PCM_CANID,Constants.LOADING_STATION_INTAKE_PCMID); + hatchPusher = new Solenoid(Constants.PCM_CANID,Constants.HATCH_SCORER_PCMID); + cargoScorer = new Solenoid(Constants.PCM_CANID,Constants.CARGO_SCORER_PCMID); + + + } + + @Override + public void periodic() { + super.periodic(); + SmartDashboard.putData("BillSolenoid",bill); + SmartDashboard.putData("HatchPushSolenoid",hatchPusher); + } + @Override + public void initDefaultCommand() { + // Set the default command for a subsystem here. + // setDefaultCommand(new MySpecialCommand()); + setDefaultCommand(new CargoManipulatorDefault()); + } +} diff --git a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/Shifter.java b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/Shifter.java index 37d7775..c47f4da 100644 --- a/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/Shifter.java +++ b/RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/subsystems/Shifter.java @@ -24,8 +24,8 @@ public class Shifter extends Subsystem { private Solenoid driveTrainShifter; public Shifter() { - driveTrainShifter = new Solenoid(Constants.PCM_CANID, Constants.DRIVETRAIN_SHIFTER_PCMID); - addChild("DriveTrainShifter",driveTrainShifter); + // driveTrainShifter = new Solenoid(Constants.PCM_CANID, Constants.DRIVETRAIN_SHIFTER_PCMID); + //addChild("DriveTrainShifter",driveTrainShifter); } @Override