diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 89e2039..c9d7a7d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -26,4 +26,38 @@ public static class OperatorConstants { public static final int LEFT_INTAKE = 0; public static final int XBOX_CONTROLLER_PORT = 0; public static final int LINE_BREAKER_PORT = 1; + +// arm constants +//TODO all the constants are subject for further tuning and assignment +public static final int frameMotorPort = 5; +public static final int rodMotorPort = 6; +public static final int throwerMotorPort = 7; + +public static final int[] kEncoderPorts = + new int[] { 4,5, + 6,7, + 8,9}; +public static final int kEncoderPPR = 256; +public static final double kEncoderDistancePerPulse = 2.0 * Math.PI / kEncoderPPR; + +public static final double kArmOffsetRads = 0.5; +public static final double kP = 1; + +public static final double kMaxAccelerationRadPerSecSquared = 10; +public static final double kMaxVelocityRadPerSecond = 3; + +public static final double kSVolts = 1; +public static final double kGVolts = 1; +public static final double kVVoltSecondPerRad = 0.5; +public static final double kAVoltSecondSquaredPerRad = 0.1; + +public enum ArmCommands { + CALIBRATE, + REST, + THROW_UP, + THROW_MID, + INTAKE +} + } + diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5af2f56..5e4ffa0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,13 +12,16 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.DriveArcade; import frc.robot.commands.DriveArcadeCustomized; import frc.robot.commands.DriveCurvature; import frc.robot.commands.DriveTank; import frc.robot.commands.GrabberOne; +import frc.robot.commands.ArmCommand; import frc.robot.subsystems.BoxGrabber; +import frc.robot.subsystems.Arm; // import frc.robot.commands.GrabberIntake; // import frc.robot.commands.GrabberOne; // import frc.robot.subsystems.BoxGrabber; @@ -32,18 +35,22 @@ */ public class RobotContainer { // The robot's subsystems and commands are defined here... + Arm arm = new Arm(); BoxGrabber boxGrabber = new BoxGrabber(); Drivetrain driveTrain = new Drivetrain(); DigitalInput digitalInput = new DigitalInput(Constants.LINE_BREAKER_PORT); // add port number in constants file - XboxController xboxController = new XboxController(Constants.XBOX_CONTROLLER_PORT); - + POVButton[] povButtons; // Replace with CommandPS4Controller or CommandJoystick if needed /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + for (int i=0; i<4; ++i) { + povButtons[i] = new POVButton(xboxController, i * 90); + } + // Configure the trigger bindings configureBindings(); // driveTrain.setDefaultCommand(new DriveArcade(driveTrain, xboxController::getRightX, xboxController::getLeftY)); @@ -66,10 +73,20 @@ public RobotContainer() { * joysticks}. */ private void configureBindings() { + // Schedule `ExampleCommand` when `exampleCondition` changes to `true` new JoystickButton(xboxController, Button.kX.value).whileTrue(new GrabberOne(boxGrabber, 0.75, digitalInput)); new JoystickButton(xboxController, Button.kY.value).whileTrue(new GrabberOne(boxGrabber, 0.75, digitalInput)); + // Arm controls + //TODO set appropriate button # for each command - 0,1,2,3 - may be inconvenient + //OR even map the actions to a different buttons like A,B,... + new JoystickButton(xboxController, Button.kA.value).whileTrue(new ArmCommand(arm,Constants.ArmCommands.CALIBRATE)); + povButtons[0].onTrue(new ArmCommand(arm, Constants.ArmCommands.INTAKE)); + povButtons[1].onTrue(new ArmCommand(arm, Constants.ArmCommands.THROW_UP)); + povButtons[2].onTrue(new ArmCommand(arm, Constants.ArmCommands.THROW_MID)); + povButtons[3].onTrue(new ArmCommand(arm, Constants.ArmCommands.REST)); + // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, // cancelling on release. @@ -81,4 +98,4 @@ private void configureBindings() { * @return the command to run in autonomous */ -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java new file mode 100644 index 0000000..3da6acb --- /dev/null +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -0,0 +1,60 @@ + +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Arm; +import frc.robot.Constants; + +public class ArmCommand extends CommandBase { + + + Arm arm; + Constants.ArmCommands command; + + public ArmCommand(Arm arm, Constants.ArmCommands comm) { + this.arm = arm; + this.command = comm; + + addRequirements(arm); + } + + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + switch (command) { + case CALIBRATE: + arm.calibrate(); + break; + case INTAKE: + arm.intake(); + break; + case THROW_UP: + arm.throwUp(); + break; + case THROW_MID: + arm.throwMid(); + break; + default: + arm.rest(); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java new file mode 100644 index 0000000..617fc95 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -0,0 +1,138 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; + +import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +// sim dependencies +import edu.wpi.first.wpilibj.simulation.EncoderSim; + +public class Arm extends SubsystemBase { + + private class Frame extends ProfiledPIDSubsystem { + private final PWMSparkMax m_frameMotor = + new PWMSparkMax(Constants.frameMotorPort); + private final Encoder m_frameEncoder = + new Encoder(Constants.kEncoderPorts[0], Constants.kEncoderPorts[1]); + private final ArmFeedforward m_frameFeedforward = + new ArmFeedforward(Constants.kSVolts, Constants.kGVolts, + Constants.kVVoltSecondPerRad, + Constants.kAVoltSecondSquaredPerRad); + + public Frame() { + super( + new ProfiledPIDController( + Constants.kP, + 0, + 0, + new TrapezoidProfile.Constraints( + Constants.kMaxVelocityRadPerSecond, + Constants.kMaxAccelerationRadPerSecSquared)), + 0); + m_frameEncoder.setDistancePerPulse(Constants.kEncoderDistancePerPulse); + } + + @Override + public void useOutput(double output, TrapezoidProfile.State setpoint) { + // Calculate the feedforward from the sepoint + double feedforward = m_frameFeedforward.calculate(setpoint.position, + setpoint.velocity); + // Add the feedforward to the PID output to get the motor output + m_frameMotor.setVoltage(output + feedforward); + } + + @Override + public double getMeasurement() { + return m_frameEncoder.getDistance() + Constants.kArmOffsetRads; + } + // Start arm at rest in neutral position + //setGoal(Constants.kArmOffsetRads); + } + + private class Rod extends ProfiledPIDSubsystem { + private final PWMSparkMax m_rodMotor = + new PWMSparkMax(Constants.rodMotorPort); + private final Encoder m_rodEncoder = + new Encoder(Constants.kEncoderPorts[2], Constants.kEncoderPorts[3]); + private final ArmFeedforward m_rodFeedforward = + new ArmFeedforward(Constants.kSVolts, Constants.kGVolts, + Constants.kVVoltSecondPerRad, + Constants.kAVoltSecondSquaredPerRad); + + public Rod() { + super( + new ProfiledPIDController( + Constants.kP, + 0, + 0, + new TrapezoidProfile.Constraints( + Constants.kMaxVelocityRadPerSecond, + Constants.kMaxAccelerationRadPerSecSquared)), + 0); + m_rodEncoder.setDistancePerPulse(Constants.kEncoderDistancePerPulse); + } + + @Override + public void useOutput(double output, TrapezoidProfile.State setpoint) { + // Calculate the feedforward from the sepoint + double feedforward = m_rodFeedforward.calculate(setpoint.position, + setpoint.velocity); + // Add the feedforward to the PID output to get the motor output + m_rodMotor.setVoltage(output + feedforward); + } + + @Override + public double getMeasurement() { + return m_rodEncoder.getDistance() + Constants.kArmOffsetRads; + } + + } + + public Arm() { + Frame frame = new Frame(); + frame.setGoal(0); + + Rod rod = new Rod(); + rod.setGoal(0); + } + + public void intake(){ + // move frame and rod to position for pickup, + // frame.setGoal(X); + // rod.setGoal(X); + // take cubicBall with claw + } + + public void throwUp(){ + // move frame and rod to position to throw up, + // throw cubicBall with claw + } + + public void throwMid(){ + // move frame and rod to position to throw mid, + // throw cubicBall with claw + } + + public void rest(){ + // move frame and rod to rest position + } + + public void calibrate() { + } + + public void stop(){ + // stop all motors (frame, rod, claw) + } + +} +