From f933abef6150989be7878275aaa445bd88b9ccb7 Mon Sep 17 00:00:00 2001 From: Carcraftz <40304589+Carcraftz@users.noreply.github.com> Date: Thu, 11 Mar 2021 10:28:38 -0500 Subject: [PATCH 1/2] Add files via upload --- Constants.java | 22 +++++ Main.java | 19 ++++ Robot.java | 92 ++++++++++++++++++ RobotContainer.java | 76 +++++++++++++++ RobotMap.java | 29 ++++++ commands/Aimbot.java | 91 ++++++++++++++++++ commands/TankDrive.java | 37 ++++++++ commands/TankDrivePID.java | 37 ++++++++ subsystems/DrivePID.java | 63 +++++++++++++ subsystems/DriveSubsystem.java | 60 ++++++++++++ subsystems/IntakeSubsystem.java | 65 +++++++++++++ subsystems/Limelight.java | 154 +++++++++++++++++++++++++++++++ subsystems/ShooterSubsystem.java | 64 +++++++++++++ 13 files changed, 809 insertions(+) create mode 100644 Constants.java create mode 100644 Main.java create mode 100644 Robot.java create mode 100644 RobotContainer.java create mode 100644 RobotMap.java create mode 100644 commands/Aimbot.java create mode 100644 commands/TankDrive.java create mode 100644 commands/TankDrivePID.java create mode 100644 subsystems/DrivePID.java create mode 100644 subsystems/DriveSubsystem.java create mode 100644 subsystems/IntakeSubsystem.java create mode 100644 subsystems/Limelight.java create mode 100644 subsystems/ShooterSubsystem.java diff --git a/Constants.java b/Constants.java new file mode 100644 index 0000000..6976bc0 --- /dev/null +++ b/Constants.java @@ -0,0 +1,22 @@ +/* + ___ __ __ _ ____ ____ __ __ _ ____ ____ + / __)/ \ ( ( \/ ___)(_ _)/ _\ ( ( \(_ _)/ ___) +( (__( O )/ /\___ \ )( / \/ / )( \___ \ + \___)\__/ \_)__)(____/ (__)\_/\_/\_)__) (__) (____/ + */ +package frc.robot; + +import edu.wpi.first.wpilibj.Joystick; + +public class Constants { + public static Joystick leftDriveStick = new Joystick(0); + public static Joystick rightDriveStick = new Joystick(1); + + final static public int driveSpeed = 1; + final static public int shooterMode = 0; // mode 0: low shooter | mode 1: high shooter + + final static public double intakeSpeed = 1; + + final static public double deadzone = 0.3; + +} diff --git a/Main.java b/Main.java new file mode 100644 index 0000000..5d3aa68 --- /dev/null +++ b/Main.java @@ -0,0 +1,19 @@ +/* +_ _ __ __ __ _ +( \/ ) / _\ ( )( ( \ +/ \/ \/ \ )( / / +\_)(_/\_/\_/(__)\_)__) +*/ + +package frc.robot; + +import edu.wpi.first.wpilibj.RobotBase; + +public final class Main { + private Main() { + } + + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/Robot.java b/Robot.java new file mode 100644 index 0000000..2ed2a5f --- /dev/null +++ b/Robot.java @@ -0,0 +1,92 @@ +/* ____ __ ____ __ ____ +( _ \ / \( _ \ / \(_ _) + ) /( O )) _ (( O ) )( +(__\_) \__/(____/ \__/ (__) +*/ + +package frc.robot; + +import frc.robot.OI; +// import frc.robot.commands.TankDrive; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.Limelight; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + + +public class Robot extends TimedRobot { + private Command m_autonomousCommand; + private RobotContainer m_robotContainer; + + + @Override + public void robotInit() { + OI oi = new OI(); + m_robotContainer = new RobotContainer(); + + } + + + @Override + public void robotPeriodic() { + CommandScheduler.getInstance().run(); + } + + + @Override + public void disabledInit() { + } + + @Override + public void disabledPeriodic() { + } + + + @Override + public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + } + + + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopInit() { + SmartDashboard.putBoolean("Drive Mode", true); + + + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + } + + @Override + public void teleopPeriodic() { + + } + + @Override + public void testInit() { + CommandScheduler.getInstance().cancelAll(); + } + + + @Override + public void testPeriodic() { + } +} + \ No newline at end of file diff --git a/RobotContainer.java b/RobotContainer.java new file mode 100644 index 0000000..adcc5c8 --- /dev/null +++ b/RobotContainer.java @@ -0,0 +1,76 @@ +/* + ____ __ ____ __ ____ ___ __ __ _ ____ __ __ __ _ ____ ____ +( _ \ / \( _ \ / \(_ _) / __)/ \ ( ( \(_ _)/ _\ ( )( ( \( __)( _ \ + ) /( O )) _ (( O ) )( ( (__( O )/ / )( / \ )( / / ) _) ) / +(__\_) \__/(____/ \__/ (__) \___)\__/ \_)__) (__)\_/\_/(__)\_)__)(____)(__\_)*/ +package frc.robot; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import frc.robot.commands.*; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.*; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.button.*; + +public class RobotContainer { + /* + * OI + */ + + public static Joystick rightJoy = new Joystick(0); + public static Joystick leftJoy = new Joystick(1); + + /* + * Subsystems + */ + + // public final static ShooterSubsystem m_ShooterSubsystem = new + // ShooterSubsystem(); + public final static Limelight m_limelight = new Limelight("limelight"); + // public final static IntakeSubsystem m_IntakeSubsystem = new + // IntakeSubsystem(); + public final static DriveSubsystem m_driveSubsystem = new DriveSubsystem(); + public final static DriveSubsystem m_driveSubsystemPID = new DrivePID(); + + /* + * Commands + */ + + // private final AutoCommand m_autoCommand = new AutoCommand(); + + public RobotContainer() { + configureButtonBindings(); + m_driveSubsystem.setDefaultCommand(new TankDrivePID()); + } + + public static Joystick getRightJoy() { + return rightJoy; + } + + public static Joystick getLeftJoy() { + return leftJoy; + } + + private void configureButtonBindings() { + GenericHID operatorGamepad = new Joystick(RobotMap.operatorGamepad); + Button shootButton = new JoystickButton(operatorGamepad, RobotMap.shootButton); + Button shooterUp = new JoystickButton(operatorGamepad, 1); + Button shooterDown = new JoystickButton(operatorGamepad, 2); + Button aimBot = new JoystickButton(operatorGamepad, 3); + Button rotatePanelButton = new JoystickButton(operatorGamepad, 5); + Button openIntake = new JoystickButton(operatorGamepad, 6); + Button closeIntake = new JoystickButton(operatorGamepad, 7); + + // public GenericHID getRightJoystick(){ return operatorGamepad; } + // public double readRightForwardAxis() { return rightJoystick.getY();} + + } + + public Command getAutonomousCommand() { + return null; + } +} diff --git a/RobotMap.java b/RobotMap.java new file mode 100644 index 0000000..5d82e24 --- /dev/null +++ b/RobotMap.java @@ -0,0 +1,29 @@ +/* ____ __ ____ __ ____ _ _ __ ____ +( _ \ / \( _ \ / \(_ _)( \/ ) / _\ ( _ \ + ) /( O )) _ (( O ) )( / \/ \/ \ ) __/ +(__\_) \__/(____/ \__/ (__) \_)(_/\_/\_/(__) */ + +package frc.robot; + +public class RobotMap { + public static int driverGamepad = 0; + public static int operatorGamepad = 2; + public static int leftStick = 0; + public static int rightStick = 0; + public static int shootButton = 7; + public static int intakeMotor = 0; + public static int shooterAngleMotor = 10; + public static int topMotor = 11; + public static int bottomMotor = 12; + + public static class activatorSolenoid { + public static int forwardChannel = 0; + public static int reverseChannel = 0; + } + + public static final int BALL_INTAKE_MOTOR = 7; + public static final int SOLENOID1_FORWARD_CHANNEL = 4; + public static final int SOLENOID1_REVERSE_CHANNEL = 5; + public static final int SOLENOID2_FORWARD_CHANNEL = 6; + public static final int SOLENOID2_REVERSE_CHANNEL = 7; +} \ No newline at end of file diff --git a/commands/Aimbot.java b/commands/Aimbot.java new file mode 100644 index 0000000..6a7f515 --- /dev/null +++ b/commands/Aimbot.java @@ -0,0 +1,91 @@ +/* + __ __ _ _ ____ __ ____ + / _\ ( )( \/ )( _ \ / \(_ _) +/ \ )( / \/ \ ) _ (( O ) )( +\_/\_/(__)\_)(_/(____/ \__/ (__) */ + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Robot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.RobotContainer; + +public class Aimbot extends CommandBase { + + public Aimbot() { + SmartDashboard.putNumber("constructed aimbot ", 1); + addRequirements(RobotContainer.m_limelight); + addRequirements(RobotContainer.m_driveSubsystem); + + } + + @Override + public void initialize() { + SmartDashboard.putNumber("Running Limelight - init", 1); + RobotContainer.m_limelight.setLED("on"); + // RobotContainer.m_limelight.driverOnlyMode(false); + + } + + @Override + public void execute() { + // PID VALUES (MUST BE TUNED) + final double kPx = -.02f; + final double kPy = -.1f; + // minimum value to actually make the robot turn (at smaller values it may not + // turn due to friction) + final double min_aim_command = .16f; + + // if a target is locked + if (RobotContainer.m_limelight.hasTarget()) { + SmartDashboard.putNumber("LIMELIGHT DISTANCE", RobotContainer.m_limelight.getDistance()); + // grab data and add to smart dashboard + SmartDashboard.putBoolean("Limelight Target", true); + double tx = RobotContainer.m_limelight.getHorizontalOffset(); + SmartDashboard.putNumber("Horizontal Offset", tx); + double ty = RobotContainer.m_limelight.getVerticalOffset(); + SmartDashboard.putNumber("Vertical Offset", ty); + double distance = RobotContainer.m_limelight.getDistance(); + SmartDashboard.putNumber("Limelight Distance", distance); + + double x_adjust; + // on large angles, ignore the minimum aim value, on smaller angles add it to + // make sure the robot moves + if (Math.abs(tx) > 10.0) { + SmartDashboard.putNumber("applying x adjust", 0); + x_adjust = kPx * -tx; + } else { + SmartDashboard.putNumber("applying x adjust", 1); + if (kPx * -tx > 0) { + x_adjust = kPx * -tx + min_aim_command; + } else { + x_adjust = kPx * -tx - min_aim_command; + } + } + if (Math.abs(tx) < 1) { + x_adjust = 0; + SmartDashboard.putNumber("on target?", 1); + } + double y_adjust = kPy * -ty; + // double y_adjust = 0; + SmartDashboard.putNumber("Pivot Adjust Val", x_adjust); + SmartDashboard.putNumber("Drive Adjust Val", y_adjust); + + // drive the robot based on these motor values + RobotContainer.m_driveSubsystem.drive(x_adjust + y_adjust, -(x_adjust - y_adjust)); + // RobotContainer.m_driveSubsystem.drive(0,min_aim_command); + + } else { + SmartDashboard.putBoolean("Limelight Target", false); + } + + } + + @Override + public boolean isFinished() { + // RobotContainer.m_limelight.setLED("off"); + // RobotContainer.m_limelight.driverOnlyMode(true); + return true; + } +} \ No newline at end of file diff --git a/commands/TankDrive.java b/commands/TankDrive.java new file mode 100644 index 0000000..88af10f --- /dev/null +++ b/commands/TankDrive.java @@ -0,0 +1,37 @@ +/* +__ __ __ _ __ _ ____ ____ __ _ _ ____ +(_ _)/ _\ ( ( \( / )( \( _ \( )/ )( \( __) + )( / \/ / ) ( ) D ( ) / )( \ \/ / ) _) + (__)\_/\_/\_)__)(__\_)(____/(__\_)(__) \__/ (____) + */ + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; +import frc.robot.RobotContainer; + +public class TankDrive extends CommandBase { + + public TankDrive() { + addRequirements(RobotContainer.m_driveSubsystem); + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + RobotContainer.m_driveSubsystem.drive(RobotContainer.getLeftJoy().getY(), RobotContainer.getRightJoy().getY()); + } + + @Override + public void end(boolean interrupted) { + } + + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/commands/TankDrivePID.java b/commands/TankDrivePID.java new file mode 100644 index 0000000..b4ad698 --- /dev/null +++ b/commands/TankDrivePID.java @@ -0,0 +1,37 @@ +/* +__ __ __ _ __ _ ____ ____ __ _ _ ____ +(_ _)/ _\ ( ( \( / )( \( _ \( )/ )( \( __) + )( / \/ / ) ( ) D ( ) / )( \ \/ / ) _) + (__)\_/\_/\_)__)(__\_)(____/(__\_)(__) \__/ (____) + */ + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; +import frc.robot.RobotContainer; + +public class TankDrivePID extends CommandBase { + + public TankDrivePID() { + addRequirements(RobotContainer.m_driveSubsystemPID); + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + RobotContainer.m_driveSubsystemPID.drive(RobotContainer.getLeftJoy().getY(), RobotContainer.getRightJoy().getY()); + } + + @Override + public void end(boolean interrupted) { + } + + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/subsystems/DrivePID.java b/subsystems/DrivePID.java new file mode 100644 index 0000000..6da24e2 --- /dev/null +++ b/subsystems/DrivePID.java @@ -0,0 +1,63 @@ +/* + +*/ + +package frc.robot.subsystems; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel; + +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class DrivePID extends SubsystemBase { + + double kP = .5; + //double kI = 1; + //double kD = 1; + double straightTolerance = .1; + + CANSparkMax left1; + CANSparkMax left2; + CANSparkMax right1; + CANSparkMax right2; + // CANSparkMax rightB; + + DifferentialDrive drive; + + public DrivePID() { + right1 = new CANSparkMax(1, CANSparkMaxLowLevel.MotorType.kBrushless); + right2 = new CANSparkMax(2, CANSparkMaxLowLevel.MotorType.kBrushless); + left1 = new CANSparkMax(3, CANSparkMaxLowLevel.MotorType.kBrushless); + left2 = new CANSparkMax(4, CANSparkMaxLowLevel.MotorType.kBrushless); + // rightM = new CANSparkMax(5, CANSparkMaxLowLevel.MotorType.kBrushless); + // rightB = new CANSparkMax(6, CANSparkMaxLowLevel.MotorType.kBrushless); + + left2.follow(left1); + // leftB.follow(leftF); + right2.follow(right1); + // rightB.follow(rightF); + leftencoder = new CANEncoder(left1); + rightencoder = new CANEncoder(right1); + + left1.getEncoder().setPosition(0.0); + right1.getEncoder().setPosition(0.0); + + drive = new DifferentialDrive(left1, right1); + } + + public void drive(double left, double right) { + double diff = Math.abs(left-right); + if(diff< straightTolerance){ + //assume driver wants to drive straught + double error = rightencoder.getVelocity()-leftencoder.getVelocity(); + double pivotAdjust = kP*error; + } + System.out.println("left: " + (left+pivotAdjust) + " right: " + (right-pivotAdjust)); + drive.tankDrive(left+pivotAdjust, right-pivotAdjust); + } + + @Override + public void periodic() { + } +} diff --git a/subsystems/DriveSubsystem.java b/subsystems/DriveSubsystem.java new file mode 100644 index 0000000..77e004d --- /dev/null +++ b/subsystems/DriveSubsystem.java @@ -0,0 +1,60 @@ +/* + ____ ____ __ _ _ ____ ____ _ _ ____ ____ _ _ ____ ____ ____ _ _ +( \( _ \( )/ )( \( __)/ ___)/ )( \( _ \/ ___)( \/ )/ ___)(_ _)( __)( \/ ) + ) D ( ) / )( \ \/ / ) _) \___ \) \/ ( ) _ (\___ \ ) / \___ \ )( ) _) / \/ \ +(____/(__\_)(__) \__/ (____)(____/\____/(____/(____/(__/ (____/ (__) (____)\_)(_/*/ + +package frc.robot.subsystems; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel; + +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class DriveSubsystem extends SubsystemBase { + + CANSparkMax left1; + CANSparkMax left2; + CANSparkMax right1; + CANSparkMax right2; + // CANSparkMax rightB; + + DifferentialDrive drive; + + public DriveSubsystem() { + right1 = new CANSparkMax(1, CANSparkMaxLowLevel.MotorType.kBrushless); + right2 = new CANSparkMax(2, CANSparkMaxLowLevel.MotorType.kBrushless); + left1 = new CANSparkMax(3, CANSparkMaxLowLevel.MotorType.kBrushless); + left2 = new CANSparkMax(4, CANSparkMaxLowLevel.MotorType.kBrushless); + // rightM = new CANSparkMax(5, CANSparkMaxLowLevel.MotorType.kBrushless); + // rightB = new CANSparkMax(6, CANSparkMaxLowLevel.MotorType.kBrushless); + + left2.follow(left1); + // leftB.follow(leftF); + right2.follow(right1); + // rightB.follow(rightF); + + /* + * OR: + * + * SpeedControllerGroup(leftF, leftM, leftB) SpeedControllerGroup(rightF, + * rightM, rightB) + * + */ + + left1.getEncoder().setPosition(0.0); + right1.getEncoder().setPosition(0.0); + + drive = new DifferentialDrive(left1, right1); + } + + public void drive(double left, double right) { + System.out.println("left: " + left + " right: " + right); + drive.tankDrive(left, right); + } + + @Override + public void periodic() { + } +} diff --git a/subsystems/IntakeSubsystem.java b/subsystems/IntakeSubsystem.java new file mode 100644 index 0000000..f8c19bc --- /dev/null +++ b/subsystems/IntakeSubsystem.java @@ -0,0 +1,65 @@ +/* + __ __ _ ____ __ __ _ ____ ____ _ _ ____ ____ _ _ ____ ____ ____ _ _ + ( )( ( \(_ _)/ _\ ( / )( __)/ ___)/ )( \( _ \/ ___)( \/ )/ ___)(_ _)( __)( \/ ) + )( / / )( / \ ) ( ) _) \___ \) \/ ( ) _ (\___ \ ) / \___ \ )( ) _) / \/ \ + (__)\_)__) (__)\_/\_/(__\_)(____)(____/\____/(____/(____/(__/ (____/ (__) (____)\_)(_/*/ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.DoubleSolenoid; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotMap; +import frc.robot.Constants; + +public class IntakeSubsystem extends SubsystemBase { + + // Two slaved motors to manage belt + private DoubleSolenoid activatorSolenoid1; + private DoubleSolenoid activatorSolenoid2; + private TalonSRX intakeMotor; + + public IntakeSubsystem() { + activatorSolenoid1 = new DoubleSolenoid(RobotMap.SOLENOID1_FORWARD_CHANNEL, RobotMap.SOLENOID1_REVERSE_CHANNEL); + activatorSolenoid2 = new DoubleSolenoid(RobotMap.SOLENOID2_FORWARD_CHANNEL, RobotMap.SOLENOID2_REVERSE_CHANNEL); + intakeMotor = new TalonSRX(7); + } + + @Override + public void periodic() { + } + + public void intakeBall() { + System.out.println("Intaking ball"); + intakeMotor.set(ControlMode.PercentOutput, 1); + } + + public void reverseIntake() { + intakeMotor.set(ControlMode.PercentOutput, Constants.intakeSpeed); + } + + public void stopSpin() { + intakeMotor.set(ControlMode.PercentOutput, 0); + } + + public void openIntake() { + System.out.println("Open Intak"); + activatorSolenoid1.set(DoubleSolenoid.Value.kForward); + activatorSolenoid2.set(DoubleSolenoid.Value.kForward); + } + + public void closeIntake() { + System.out.println("Close intake"); + activatorSolenoid1.set(DoubleSolenoid.Value.kReverse); + activatorSolenoid2.set(DoubleSolenoid.Value.kReverse); + + } + + public void disableIntake() { + activatorSolenoid1.set(DoubleSolenoid.Value.kOff); + activatorSolenoid2.set(DoubleSolenoid.Value.kOff); + } +} diff --git a/subsystems/Limelight.java b/subsystems/Limelight.java new file mode 100644 index 0000000..7f12990 --- /dev/null +++ b/subsystems/Limelight.java @@ -0,0 +1,154 @@ +/* __ __ _ _ ____ __ __ ___ _ _ ____ +( ) ( )( \/ )( __)( ) ( )/ __)/ )( \(_ _) +/ (_/\ )( / \/ \ ) _) / (_/\ )(( (_ \) __ ( )( +\____/(__)\_)(_/(____)\____/(__)\___/\_)(_/ (__) */ + +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import edu.wpi.first.networktables.*; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class Limelight extends SubsystemBase { + + private String networkTableName; + + NetworkTable table; + + /** + * Constructs a new Limelight Object with network table name (to connect to the + * correct limelight). Default is "limelight" + */ + + public Limelight(String networkTableName) { + + this.networkTableName = networkTableName; + + table = NetworkTableInstance.getDefault().getTable(this.networkTableName); + + SmartDashboard.putNumber("Limelight object contructed", table.getEntry("tl").getDouble(0)); + + } + + // set pipeline number (0-9 configured on limelight web dashboard) + public void setPipeline(int pipelinenumber) throws IllegalArgumentException { + + if (pipelinenumber > 9 || pipelinenumber < 0) { + throw new IllegalArgumentException(); + } + + table.getEntry("pipeline").setNumber(pipelinenumber); + + } + + // set limelight LED to on off or blink + public void setLED(String status) throws IllegalArgumentException { + + if (status.equalsIgnoreCase("on")) { + table.getEntry("ledMode").setNumber(3); + } + + else if (status.equalsIgnoreCase("off")) { + table.getEntry("ledMode").setNumber(1); + } + + else if (status.equalsIgnoreCase("blink")) { + table.getEntry("ledMode").setNumber(2); + } + + else { + throw new IllegalArgumentException(); + } + + } + + // set limelight camera to driver only mode (increases exposure turns off vision + // processing) (true or false) + public void driverOnlyMode(boolean drivermode) { + + if (drivermode) { + table.getEntry("camMode").setNumber(1); + } else { + table.getEntry("camMode").setNumber(0); + } + + } + + // take a snapshot, can be viewed later from the limelight web dashboard + public void takeSnapshot() { + + table.getEntry("snapshot").setNumber(1); + table.getEntry("snapshot").setNumber(0); + + } + + // Whether the limelight has any valid targets + public boolean hasTarget() { + + double value = table.getEntry("tv").getDouble(0); + if (value == 0) { + return false; + } else if (value == 1) { + return true; + } else { + System.out.println("Error in getting target data; Limelight may not be connected"); + return false; + } + + } + + // Horizontal Offset From Crosshair To Target (LL1: -27 degrees to 27 degrees | + // LL2: -29.8 to 29.8 degrees) + public double getHorizontalOffset() { + return table.getEntry("tx").getDouble(0); + } + + // Vertical Offset From Crosshair To Target (LL1: -20.5 degrees to 20.5 degrees + // | LL2: -24.85 to 24.85 degrees) + public double getVerticalOffset() { + return table.getEntry("ty").getDouble(0); + } + + // Target Area (0% of image to 100% of image) + public double getTargetArea() { + return table.getEntry("ta").getDouble(0); + } + + // Skew or rotation (-90 degrees to 0 degrees) + public double getSkew() { + return table.getEntry("ts").getDouble(0); + } + + // The pipeline’s latency contribution (ms) Add at least 11ms for image capture + // latency. + public double getLatency() { + return table.getEntry("tl").getDouble(0); + } + + // Gets the number of corners the limelight detects (WIP) + public double getNumOfCorners() { + // return + // NetworkTableInstance.getDefault().getTable(networktablename).getEntry("tcornx").getDoubleArray().length(); + return 4; + } + + // Vertical sidelength of the rough bounding box (0 - 320 pixels) + public double getVerticalSidelength() { + return table.getEntry("tvert").getDouble(0); + } + + // Horizontal sidelength of the rough bounding box (0 - 320 pixels) + public double getHorizontalSideLength() { + return table.getEntry("thor").getDouble(0); + } + + public double getDistance() { + double h_goal = 13; // Height of the goal in inches + double h_limeyboy = 5; + double shooter_Angle = 12.5; // fixed shooter angle. TODO: experimentally determine this + double distanceLimelight = (h_goal - h_limeyboy) / Math.tan(Math.toRadians(shooter_Angle + getVerticalOffset())); + return distanceLimelight; + } + +} \ No newline at end of file diff --git a/subsystems/ShooterSubsystem.java b/subsystems/ShooterSubsystem.java new file mode 100644 index 0000000..0bd93ff --- /dev/null +++ b/subsystems/ShooterSubsystem.java @@ -0,0 +1,64 @@ +/* + ____ _ _ __ __ ____ ____ ____ ____ _ _ ____ ____ _ _ ____ ____ ____ _ _ +/ ___)/ )( \ / \ / \(_ _)( __)( _ \ / ___)/ )( \( _ \/ ___)( \/ )/ ___)(_ _)( __)( \/ ) +\___ \) __ (( O )( O ) )( ) _) ) / \___ \) \/ ( ) _ (\___ \ ) / \___ \ )( ) _) / \/ \ +(____/\_)(_/ \__/ \__/ (__) (____)(__\_) (____/\____/(____/(____/(__/ (____/ (__) (____)\_)(_/ +*/ +package frc.robot.subsystems; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel; +//import edu.wpi.first.wpilibj.command.Command; +//import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.OI; +import frc.robot.RobotMap; + +//import com.ctre.phoenix.motorcontrol.ControlMode; +//import com.ctre.phoenix.motorcontrol.can.TalonSRX; + +public class ShooterSubsystem extends SubsystemBase { + public static ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem(); + + private OI m_oi; + // TalonSRX neo = new TalonSRX(1); + // TalonSRX topMotor = new TalonSRX(7); + // TalonSRX bottomMotor = new TalonSRX(8); + CANSparkMax neo = new CANSparkMax(RobotMap.shooterAngleMotor, CANSparkMaxLowLevel.MotorType.kBrushless); + CANSparkMax topMotor = new CANSparkMax(RobotMap.topMotor, CANSparkMaxLowLevel.MotorType.kBrushless); + CANSparkMax bottomMotor = new CANSparkMax(RobotMap.bottomMotor, CANSparkMaxLowLevel.MotorType.kBrushless); + + public void shooterUp() { + neo.set(.1); + } + + public void shooterDown() { + neo.set(-.1); + } + + public void shoot() { + // top motor counterclockwise, bottom motor clockwise @ full speed + topMotor.set(1); + bottomMotor.set(-1); + } + + public void setNeoSpeed(double speed) { + neo.set(speed); + } + + public void setTopMotorSpeed(double speed) { + topMotor.set(speed); + } + + public void setBottomMotorSpeed(double speed) { + bottomMotor.set(speed); + } + + // @Override + // default void setDefaultCommand(Command defaultCommand){ + // // set the default command + // // setDefaultCommand (new mySpecialCommand()); + // CommandScheduler.getInstance().setDefaultCommand(this, defaultCommand); + // } + +} From d70a336044e91c97e6523d21ebcc2b42828a3479 Mon Sep 17 00:00:00 2001 From: Carcraftz <40304589+Carcraftz@users.noreply.github.com> Date: Thu, 11 Mar 2021 10:37:37 -0500 Subject: [PATCH 2/2] Fix max --- subsystems/DrivePID.java | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/subsystems/DrivePID.java b/subsystems/DrivePID.java index 6da24e2..2fa931b 100644 --- a/subsystems/DrivePID.java +++ b/subsystems/DrivePID.java @@ -52,9 +52,14 @@ public void drive(double left, double right) { //assume driver wants to drive straught double error = rightencoder.getVelocity()-leftencoder.getVelocity(); double pivotAdjust = kP*error; + //if the joystick falls within this tolerance we'll just take the average value of the two joysticks and apply to both sides + double averageforward = (left+right)/2; + System.out.println("left: " + (averageforward+pivotAdjust) + " right: " + (averageforward-pivotAdjust)); + drive.tankDrive(averageforward+pivotAdjust, averageforward-pivotAdjust); + }else{ + drive.tankDrive(left,right); } - System.out.println("left: " + (left+pivotAdjust) + " right: " + (right-pivotAdjust)); - drive.tankDrive(left+pivotAdjust, right-pivotAdjust); + } @Override