Skip to content

Commit

Permalink
Subsystems (#16)
Browse files Browse the repository at this point in the history
At this point we are using shooter like develop, so even though I haven't reviewed all this code I'm going to merge it. Let's actually use develop hooray.

* Recreate shooter branch from refactored Ethan code

* Skeleton `shoot()` method, some cleanup

* Add hood SparkMax

* Remember to add a constant 😳

* PID testing

* editing jetson socket so it's using constants instead of bigdata

* add networktables

* Add Jetson NT testing code

* Update NetworkTables data to match vision code

* Clean up

* editing jetson socket so it's using constants instead of bigdata

* Update NetworkTables data to match vision code

* Clean up

* Add httpcamera for testing camera stream

* Update Jetson stuff

* Jetson refactoring

* Add velocity closed loop testing code

* Jetson restructuring, `periodic()` skeleton code

* Rename shooter to turret

Shooting has been delegated to internals!

* Jetson camera wrapper

* added elevator and tank implementation

* Tank drive code, untested

* add elevator config

* reviewed added code, default commands added

* elevator commands and controller setup

* Working tank and elevator subsystems

* overhaul subsystem structure + overhaul config file

* fix end method in dtcommand

* improved handling of elevatorUpIsPositive const

* change elevator command structure

* put in config txt thing again

* Use `WPI_TalonSRX`, `DifferentialDrive`

* Use builtin `squareInput`

* Add third set of Talons

* Remove `DifferentialDrive`

* Revert "Add third set of Talons"

This reverts commit 1b5ea6d.

* add joystick code

* Add joystick capability

* Add controller switching

* code for testing gyro, ecoder, limit switch, and color+distance sensor. Supposed to print values from sensors. Need to import rev library.

* sensor testing code

* skeleton code for internal subsystem. Includes 2 color sensors, a talon (will replace w/ cim), a method that determines when to activate/deactivate motor based on color.

* Merge commit part 2

* Internals refactoring

* Initiate InternalSubsystem in RobotContainer

* shooter request updates

* added a skeleton method for launching ball into shooter

* Add `turntableAligned()` for internals

* Change initialization to constructor arguments

* Skeleton climb code, cleanup

* Intake skeleton

* Use intake enum, add intake commands

* Reformat to 4 spaces

Finally, my master plan is complete 😈

In all seriousness, all the new files being created in each branch were all using 4 space indents and the constant refactorings to preserve an unliked indent size inherited from 2 (3?) years ago was getting annoying. 4 space is the future!

* Initialize new subsystems in RobotContainer

* Fix tank spacing, better comments

* added motorport2 constant

* added a ballDetected method, updated fields to include another motor, undefined ball count method

* Restructure intake position, PID constant splitting

* Implement new climb design

* updateBallCount

* Intake / internals integration

* merge

* Ball count tracking code

* Jetson constants organization, better reject logic

* Set up shuffleboard for PID testing

* Fix internals motor flow

* Use enum for turret mode

* got rid of TODOs

* Roughly implement the climb plan

It's pretty complex.

* 775 instead of NEO for turret hood

* Update internals with IR sensors

* Use command groups for climb

* Implement module states for turret

This is so that a) drivers know what the status of the shooter is and b) internals can reject quicker (on orange) instead of waiting for a perfectly lined up shot.

* Final-ish button bindings

* Final rebase commit

* `GRTSubsystem` changes, subsystem implementation

* Implement driver intake running

* Non-conflicting motor ports

* New constants, third tank NEO

* Turntable blind spot logic, soft limits

* Degree conversion constant stubs

* Remove mjpg prefix in jetson

* Working vision :D

* Temporary intake + DT testing code

The drivetrain and intake rollers work as expected, but the intake deploy was not moving the motor upwards (despite the percent output being set correctly). I'd advise whoever's testing this tomorrow to see if running the current code and pressing the A button causes the talon to begin blinking green (positive percent output); if it does, then it's a motor issue, otherwise its a code issue (which is weird, because `deploy.get()` seems to say that the deploy controller is indeed being set to the proper percent output).

* Replace exit with staging sensor

This should work because staging initially stops the ball the moment it detects it (when the top of the ball is at the sensor). While staging still detects the ball (for the entire diameter of the ball as it moves up) we keep spinning the top, and as long as the bottom of the ball is still seen by staging when the top of the ball reaches the turret this code should work. We can repurpose the `shotRequested = false` code which runs when the ball leaves internals to also handle ball count decrementation.

* Testing of subsystems, lowered periodic loop times, moved localization to thread

* Enforce style

* Better `ModuleState` names

* Jetson reformatting

* Fix internals current drawn method

* More subsystem testing and some refactoring

* Enforce style again.

* Attempt turret `r` and `theta` state system

* Bare bones intake testing

* Internals color sensor thread

* Measure color constants, reduce IO waste

* More testing setup

* Upgrade to WPILib 2022.3.1

* Update encoder value; comment out internals

* Button binding fix, use integrated encoders for DT

Also, experimental style changes! Unsure how I like the comma separated declaration, but it is certainly less wordy than repeating `private final JoystickButton` 8 times.

* Intake, internals, turret testing

- Commented out PowerController (it was causing power drops and stuttering, will test causes and try fixes later)
- Bind turntable and hood to mech controller joysticks
- Bind left trigger to internals, right to intake
- Preliminary internals restructuring to replace I2C onboard color sensor

* Boolean state for storage rejection checking

* More internals testing

* Internals testing part 2 -- dummy shoot method

It works half of the time! See slack for details about inconsistencies

* Shooter testing

- Bind mech y to turn on flywheel, mech b to turn off
- Manual override of internals motors on mech triggers

Co-authored-by: pinkbluesky <[email protected]>
Co-authored-by: unknown <[email protected]>
Co-authored-by: eggaskin <[email protected]>
Co-authored-by: Ayeeti <[email protected]>
Co-authored-by: Ippyb <[email protected]>
  • Loading branch information
6 people authored Feb 18, 2022
1 parent caa612c commit 07aba7c
Show file tree
Hide file tree
Showing 20 changed files with 2,044 additions and 648 deletions.
2 changes: 1 addition & 1 deletion build.gradle
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@ import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO

plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2022.1.1"
id "edu.wpi.first.GradleRIO" version "2022.3.1"
}

sourceCompatibility = JavaVersion.VERSION_11
Expand Down
2 changes: 1 addition & 1 deletion src/main/deploy/config.txt
Original file line number Diff line number Diff line change
Expand Up @@ -44,4 +44,4 @@ outtake_speed=.9

# limit switch variables (currently hardcoded, ignore these)
top_switch=0
bottom_switch=1
bottom_switch=1
56 changes: 48 additions & 8 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,8 @@

package frc.robot;

import edu.wpi.first.wpilibj.util.Color;

/**
* The Constants class provides a convenient place for teams to hold robot-wide
* numerical or boolean constants. This class should not be used for any other
Expand All @@ -16,17 +18,55 @@
*/
public final class Constants {
public static final class TankConstants {
public static final int fLeftMotorPort = 14;
public static final int bLeftMotorPort = 15;
public static final int fLeftMotorPort = 4;
public static final int mLeftMotorPort = 5;
public static final int bLeftMotorPort = 6;

public static final int fRightMotorPort = 9;
public static final int mRightMotorPort = 8;
public static final int bRightMotorPort = 7;
}

public static final class TurretConstants {
public static final int turntablePort = 17;
public static final int hoodPort = 16;
public static final int flywheelPort = 15;
}

public static final class InternalConstants {
public static final double RAISE_POW = 0.8;

public static final int motorPortBottom = 13;
public static final int motorPortTop = 14;

public static final int fRightMotorPort = 2;
public static final int bRightMotorPort = 1;
public static final int entranceIRPort = 0;
public static final int stagingIRPort = 1;
public static final int exitIRPort = 3;
}

public static final class ElevatorConstants {
public static final int mainMotorPort = 5;
public static final int followMotorPort = 6;
public static final class IntakeConstants {
public static final int intakePort = 12;
public static final int deploymentPort = 11;
}

public static final class ClimbConstants {
public static final int sixMotorPort = 0;
public static final int sixBrakePort = 1;

public static final int tenMotorPort = 3;
public static final int tenBrakePort = 2;

public static final int tenLeftSolenoidPort = 18;
public static final int tenRightSolenoidPort = 18;

public static final int fifteenLeftPort = 10;
public static final int fifteenRightPort = 19;
}

public static final boolean elevatorUpIsPositive = true;
public static final class JetsonConstants {
public static final int turretCameraPort = 1181;
public static final int intakeCameraPort = 1182;
public static final int jetsonPort = 1337;
public static final String jetsonIP = "10.1.92.94";
}
}
18 changes: 4 additions & 14 deletions src/main/java/frc/robot/GRTSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -6,16 +6,13 @@
* A convenience class wrapping SubsystemBase for brownout and other shared subsystem logic.
*/
public abstract class GRTSubsystem extends SubsystemBase {
protected int currentLimit;
protected final double minCurrent;

/**
* Creates a GRTSubsystem from a given initial current limit and minimum current.
* @param currentLimit The initial (maximum) current limit.
* Creates a GRTSubsystem from the given minimum current.
* @param minCurrent The minimum current.
*/
public GRTSubsystem(int currentLimit, double minCurrent) {
this.currentLimit = currentLimit;
public GRTSubsystem(double minCurrent) {
this.minCurrent = minCurrent;
}

Expand All @@ -27,14 +24,6 @@ public double getMinCurrent() {
return minCurrent;
}

/**
* Gets the subsystem's current current limit.
* @return The current limit.
*/
public int getCurrentLimit() {
return currentLimit;
}

/**
* Gets the total current being drawn by the subsystem. Implementers should use `PowerController.getCurrentDrawnFromPDH`
* with all the subsystem's motor controller ports.
Expand All @@ -43,7 +32,8 @@ public int getCurrentLimit() {
abstract public double getTotalCurrentDrawn();

/**
* Sets the subsystem's current limit.
* Sets the subsystem's current limit. Implementers should split the subsystem's limit between their motor controllers and
* call `setSmartCurrentLimit()` or the Talon equivalent to enforce the limit.
* @param limit The current limit (in amps) for the subsystem.
*/
abstract public void setCurrentLimit(double limit);
Expand Down
18 changes: 9 additions & 9 deletions src/main/java/frc/robot/Main.java
Original file line number Diff line number Diff line change
Expand Up @@ -12,14 +12,14 @@
* call.
*/
public final class Main {
private Main() {}
private Main() {}

/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}
152 changes: 77 additions & 75 deletions src/main/java/frc/robot/Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
package frc.robot;

import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;

Expand All @@ -15,84 +16,85 @@
* project.
*/
public class Robot extends TimedRobot {
private Command autonomousCommand;

private RobotContainer robotContainer;

/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
@Override
public void robotInit() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
robotContainer = new RobotContainer();
}

/**
* This function is called every robot packet, no matter the mode. Use this for items like
* diagnostics that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
* SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
// commands, running already-scheduled commands, removing finished or interrupted commands,
// and running subsystem periodic() methods. This must be called from the robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();

// Calculate current limits for subsystems
robotContainer.getPowerController().calculateLimits();
}

/** This function is called once each time the robot enters Disabled mode. */
@Override
public void disabledInit() {}

@Override
public void disabledPeriodic() {}

/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
autonomousCommand = robotContainer.getAutonomousCommand();

// schedule the autonomous command (example)
if (autonomousCommand != null) {
autonomousCommand.schedule();
private Command autonomousCommand;

private RobotContainer robotContainer;

/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
@Override
public void robotInit() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
robotContainer = new RobotContainer();
LiveWindow.disableAllTelemetry();
}
}

/** This function is called periodically during autonomous. */
@Override
public void autonomousPeriodic() {}

@Override
public void teleopInit() {
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (autonomousCommand != null) {
autonomousCommand.cancel();

/**
* This function is called every robot packet, no matter the mode. Use this for items like
* diagnostics that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
* SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
// commands, running already-scheduled commands, removing finished or interrupted commands,
// and running subsystem periodic() methods. This must be called from the robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();

// Calculate current limits for subsystems
//robotContainer.getPowerController().calculateLimits();
}
}

/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {}
/** This function is called once each time the robot enters Disabled mode. */
@Override
public void disabledInit() {}

@Override
public void disabledPeriodic() {}

/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
autonomousCommand = robotContainer.getAutonomousCommand();

// schedule the autonomous command (example)
if (autonomousCommand != null) {
autonomousCommand.schedule();
}
}

@Override
public void testInit() {
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
}
/** This function is called periodically during autonomous. */
@Override
public void autonomousPeriodic() {}

@Override
public void teleopInit() {
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (autonomousCommand != null) {
autonomousCommand.cancel();
}
}

/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {}

@Override
public void testInit() {
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
}

/** This function is called periodically during test mode. */
@Override
public void testPeriodic() {}
/** This function is called periodically during test mode. */
@Override
public void testPeriodic() {}
}
Loading

0 comments on commit 07aba7c

Please sign in to comment.