thomas/2022-flight-code
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

BBBBBBBBBBBBBBBBBBBBBBBBBBBB

Browse source
This commit is contained in:
Thomas Muller 2022-03-12 08:50:16 -05:00
parent 3058a6a714
commit bbfa034a0c
24 changed files with 1071 additions and 217 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

75
src/main/java/frc/robot/Constants.java
View file

@ -1,53 +1,64 @@
// 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;
import edu.wpi.first.math.geometry.Translation2d;
/**
* 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 purpose. All constants should be declared
* globally (i.e. public static). Do not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
// public static int LF_DRIVE = 0;
// public static int LF_TURN = 1;
// public static int RF_DRIVE = 2;
// public static int RF_TURN = 3;
// public static int LB_DRIVE = 4;
// public static int LB_TURN = 5;
// public static int RB_DRIVE = 6;
// public static int RB_TURN = 7;
public static int LF_DRIVE = 1;
public static int LF_TURN = 2;
public static int RF_DRIVE = 3;
public static int RF_TURN = 4;
public static int LB_DRIVE = 5;
public static int LB_TURN = 6;
public static int RB_DRIVE = 7;
public static int RB_TURN = 8;
public static int LF_DRIVE = 2;
public static int LF_TURN = 1;
public static int RF_DRIVE = 4;
public static int RF_TURN = 3;
public static int LB_DRIVE = 6;
public static int LB_TURN = 5;
public static int RB_DRIVE = 8;
public static int RB_TURN = 7;
public static int SHOOTER = 10;
public static int GATHER_LOW = 20;
public static int GATHER_HIGH_BACK = 21;
public static int GATHER_HIGH_FRONT = 22;
public static int SHEAR_LEFT = 30;
public static int SHEAR_RIGHT = 31;
public static int LF_POD_ZERO_DIO = 0;
public static int RF_POD_ZERO_DIO = 1;
public static int LB_POD_ZERO_DIO = 2;
public static int RB_POD_ZERO_DIO = 3;
public static int GATHER_LOW_DIO = 4;
public static int GATHER_HIGH_DIO = 5;
public static double TURN_P = 1.0;
public static double TURN_I = 0.0;
public static double TURN_D = 0.0;
public static double SHEAR_P = 1.0;
public static double SHEAR_I = 0.0;
public static double SHEAR_D = 0.0;
public static double JOY_DRIVE_EXPO = 5.0;
public static double JOY_TURN_EXPO = 5.0;
public static double POD_COUNTS_PER_DEG = 6656.0;
public static double SHOOTER_RPM = 1.0;
public static double SHOOTER_TARGET_ERROR = 0.1;
public static double SHEAR_TOLERANCE = 0.1;
// 2048 CPS * 2 (quadrature)
public static double SHOOTER_COUNTS_PER_REV = 2048.0 * 2.0;
// 2048 CPS * 2 (quadrature) * 72 / 44 ratio
public static double POD_COUNTS_PER_REV = 2048.0 * 2.0 * 72.0 / 44.0;
public static double SHEAR_COUNTS_PER_REV = 2048.0 * 2.0 * 100.0;
// drive full speed meters
public static double DRIVE_MAX_SPEED = 5.0;
public static double DRIVE_MAX_SPEED = 9.144;
// public static double DRIVE_MAX_SPEED = 1.0;
// meters
public static double swerveXFromCenter = 1.0;
public static double swerveYFromCenter = 2.0;
public static double swerveXFromCenter = 0.2794;
public static double swerveYFromCenter = 0.3175;
public static Translation2d LF_TRANSLATION = new Translation2d(swerveXFromCenter, swerveYFromCenter);
public static Translation2d RF_TRANSLATION = new Translation2d(swerveXFromCenter, -swerveYFromCenter);

25
src/main/java/frc/robot/Robot.java
View file

@ -15,10 +15,31 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler;
* project.
*/
public class Robot extends TimedRobot {
// private enum StartAngle {
// LEFT("Left"),
// CENTER("Center"),
// RIGHT("Right");
// private final String name;
// private String getDashName() { return name; }
// StartAngle(String name) { this.name = name; }
// }
private Command m_autonomousCommand;
// public SendableChooser<StartAngle> m_startAngle;
private RobotContainer m_robotContainer;
public Robot() {
// m_startAngle = new SendableChooser<StartAngle>();
// m_startAngle.setDefaultOption(StartAngle.CENTER.getDashName(), StartAngle.CENTER);
// for (StartAngle angle : StartAngle.values()) {
// m_startAngle.addOption(angle.toString(), angle);
// }
// SmartDashboard.putData("Start angle", m_startAngle);
}
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
@ -53,7 +74,9 @@ public class Robot extends TimedRobot {
}
@Override
public void disabledPeriodic() {}
public void disabledPeriodic() {
// System.out.println(m_robotContainer.m_exampleSubsystem.gyro.getAngle());
}
/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override

56
src/main/java/frc/robot/RobotContainer.java
View file

@ -5,10 +5,24 @@
package frc.robot;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PowerDistribution;
import edu.wpi.first.wpilibj.XboxController;
import frc.robot.commands.ExampleCommand;
import frc.robot.subsystems.ExampleSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc.robot.commands.Auto;
import frc.robot.commands.Blow;
import frc.robot.commands.DriveWithJoysticks;
import frc.robot.commands.SetShear;
import frc.robot.commands.SetShooterSpeed;
import frc.robot.commands.ShearGoBrr;
import frc.robot.commands.Shoot;
import frc.robot.commands.Succ;
import frc.robot.commands.ZeroGyro;
import frc.robot.subsystems.Chassis;
import frc.robot.subsystems.Gatherer;
import frc.robot.subsystems.PotatoCannon;
import frc.robot.subsystems.Shear;
/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
@ -18,19 +32,24 @@ import edu.wpi.first.wpilibj2.command.Command;
*/
public class RobotContainer {
// The robot's subsystems and commands are defined here...
public static XboxController s_driver = new XboxController(0);
public static XboxController s_manip = new XboxController(1);
public static PowerDistribution s_pdp = new PowerDistribution();
private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
public static Joystick s_driver = new Joystick(0);
public static Joystick s_manip = new Joystick(1);
private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem);
public final Chassis m_chassis = new Chassis();
public final PotatoCannon m_potatoCannon = new PotatoCannon();
public final Gatherer m_gatherer = new Gatherer();
public final Shear m_shear = new Shear();
private final Auto m_autoCommand = new Auto(m_chassis, m_potatoCannon, m_gatherer);
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
m_exampleSubsystem.setDefaultCommand(m_autoCommand);
m_chassis.setDefaultCommand(new DriveWithJoysticks(m_chassis));
}
/**
@ -39,7 +58,28 @@ public class RobotContainer {
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
* edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {}
private void configureButtonBindings() {
new JoystickButton(s_manip, 4).whenPressed(new SetShooterSpeed(m_potatoCannon, Constants.SHOOTER_RPM));
new JoystickButton(s_manip, 3).whenPressed(new SetShooterSpeed(m_potatoCannon, Constants.SHOOTER_RPM/3.0*2.0));
new JoystickButton(s_manip, 1).whenPressed(new SetShooterSpeed(m_potatoCannon, 0.4));
new JoystickButton(s_manip, 2).whenPressed(new SetShooterSpeed(m_potatoCannon, 0.0));
new JoystickButton(s_driver, 6).whileHeld(new Succ(m_gatherer));
new JoystickButton(s_driver, 8).whileHeld(new Blow(m_gatherer));
new JoystickButton(s_driver, 10).whenPressed(new ZeroGyro(m_chassis));
new JoystickButton(s_manip, 5).whileHeld(new Shoot(m_potatoCannon, m_gatherer));
new JoystickButton(s_manip, 9).whenPressed(new SetShear(m_shear, 90.0));
// new JoystickButton(s_manip, 10).whenPressed(new SequentialCommandGroup(
// new SetShear(m_shear, 90.0),
// new SetShear(m_shear, 45.0),
// new SetShear(m_shear, -45.0)
// ));
new JoystickButton(s_manip, 10).whileHeld(new ShearGoBrr(m_shear));
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.

51
src/main/java/frc/robot/SwervePod.java Normal file
View file

@ -0,0 +1,51 @@
package frc.robot;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
import com.ctre.phoenix.motorcontrol.can.TalonFX;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
public class SwervePod {
public TalonSRX turn;
public TalonFX drive;
public SwervePod(int turn_id, int drive_id) {
turn = new TalonSRX(turn_id);
drive = new TalonFX(drive_id);
turn.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative);
turn.setSensorPhase(true);
turn.config_kP(0, Constants.TURN_P);
turn.config_kI(0, Constants.TURN_I);
turn.config_kD(0, Constants.TURN_D);
// drive.configPeakCurrentDuration(10);
drive.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 15, 0, 0.01));
// drive.enableCurrentLimit(true);ddp
}
/**
* Sets a pod to a specified angle and velocity
*
* @param pod: The pod to move
* @param angle: Pod angle in degrees
* @param velocity: Pod velocity in meters per second
*/
public void set(double angle, double velocity) {
// TODO: I can't remember if we waited for the angle target to be met before we
// start to apply the drive velocity, look into that
double drivePercent = velocity / Constants.DRIVE_MAX_SPEED;
if(Math.abs(drivePercent) > 0.05)
turn.set(ControlMode.Position, angle / 360.0 * Constants.POD_COUNTS_PER_REV);
drive.set(ControlMode.PercentOutput, drivePercent);
}
public void zeroEncoder() {
turn.setSelectedSensorPosition(0);
}
public double getAngle() {
return turn.getSelectedSensorPosition() / Constants.POD_COUNTS_PER_REV * 360.0;
}
}

13
src/main/java/frc/robot/Utils.java Normal file
View file

@ -0,0 +1,13 @@
package frc.robot;
public class Utils {
public static double joyExpo(double in, double exp) {
double out;
out = Math.exp(Math.abs(in) * exp) - 1;
out = out / (Math.exp(exp) - 1);
out = Math.signum(in) * out;
return out;
}
}

65
src/main/java/frc/robot/commands/Auto.java Normal file
View file

@ -0,0 +1,65 @@
package frc.robot.commands;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.Chassis;
import frc.robot.subsystems.Gatherer;
import frc.robot.subsystems.PotatoCannon;
public class Auto extends CommandBase {
private Chassis m_chassis;
private PotatoCannon m_cannon;
private Gatherer m_gatherer;
private double m_startTime;
public Auto(Chassis chassis, PotatoCannon cannon, Gatherer gatherer) {
addRequirements(chassis);
addRequirements(cannon);
addRequirements(gatherer);
m_chassis = chassis;
m_cannon = cannon;
m_gatherer = gatherer;
}
@Override
public void initialize() {
m_startTime = Timer.getFPGATimestamp();
}
@Override
public void execute() {
double curTime = Timer.getFPGATimestamp();
double time = curTime - m_startTime;
m_cannon.set(0.95);
if(time > 7.0) {
// System.out.println("Stop");
m_chassis.driveFR(0.0, 0.0, 0.0);
m_cannon.set(0);
m_gatherer.stop();
} else if(time > 4.0) {
// System.out.println("Move");
ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
2.0,
0.0,
0.0,
Rotation2d.fromDegrees(0.0));
m_chassis.drive(speeds);
// m_chassis.driveFR(2.0, 0.0, 0.0);
} else if(time > 2.0) {
// System.out.println("Shoot");
m_gatherer.shoot(false);
}
}
@Override
public void end(boolean interrupted) {
}
@Override
public boolean isFinished() {
return false;
}
}

32
src/main/java/frc/robot/commands/Blow.java Normal file
View file

@ -0,0 +1,32 @@
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.Gatherer;
public class Blow extends CommandBase {
private Gatherer m_gatherer;
public Blow(Gatherer gatherer) {
addRequirements(gatherer);
m_gatherer = gatherer;
}
@Override
public void initialize() {
m_gatherer.eject();
}
@Override
public void execute() {
}
@Override
public void end(boolean interrupted) {
m_gatherer.stop();
}
@Override
public boolean isFinished() {
return false;
}
}

51
src/main/java/frc/robot/commands/DriveWithJoysticks.java Normal file
View file

@ -0,0 +1,51 @@
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.Constants;
import frc.robot.RobotContainer;
import frc.robot.Utils;
import frc.robot.subsystems.Chassis;
public class DriveWithJoysticks extends CommandBase {
private final Chassis m_chassis;
public DriveWithJoysticks(Chassis chassis) {
addRequirements(chassis);
m_chassis = chassis;
}
@Override
public void initialize() {
// m_chassis.zeroPodEncoders();
// m_chassis.zeroGyro();
}
@Override
public void execute() {
// Get joystick values
double x = RobotContainer.s_driver.getY() * 2.0;
double y = RobotContainer.s_driver.getX() * 2.0;
double theta = RobotContainer.s_driver.getZ() * 1.5;
// Apply exponential to allow finer driver control
x = Utils.joyExpo(x, Constants.JOY_DRIVE_EXPO);
y = Utils.joyExpo(y, Constants.JOY_DRIVE_EXPO);
theta = Utils.joyExpo(theta, Constants.JOY_DRIVE_EXPO);
// TODO: Move this crap
// double total = 0.0;
// for(int i = 0; i < 16; i++)
// total += RobotContainer.s_pdp.getCurrent(i);
// System.out.println("A: " + total + ", W: " + RobotContainer.s_pdp.getTotalPower());
m_chassis.driveFR(x, y, theta);
}
@Override
public void end(boolean interrupted) {}
@Override
public boolean isFinished() {
return false;
}
}

117
src/main/java/frc/robot/commands/ExampleCommand.java
View file

@ -1,117 +0,0 @@
// 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 frc.robot.Constants;
import frc.robot.RobotContainer;
import frc.robot.subsystems.ExampleSubsystem;
import com.ctre.phoenix.motorcontrol.ControlMode;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj2.command.CommandBase;
/** An example command that uses an example subsystem. */
public class ExampleCommand extends CommandBase {
@SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
private final ExampleSubsystem m_subsystem;
private SwerveDriveKinematics m_kinematics;
/**
* Creates a new ExampleCommand.
*
* @param subsystem The subsystem used by this command.
*/
public ExampleCommand(ExampleSubsystem subsystem) {
m_subsystem = subsystem;
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(subsystem);
m_kinematics = new SwerveDriveKinematics(Constants.LF_TRANSLATION, Constants.RF_TRANSLATION, Constants.LB_TRANSLATION, Constants.RB_TRANSLATION);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_subsystem.lf_turn.setSelectedSensorPosition(0);
System.out.println("Cmd Init");
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double x = RobotContainer.s_driver.getLeftY();
double y = RobotContainer.s_driver.getLeftX();
double theta = RobotContainer.s_driver.getRightX();
x = joyExpo(x, Constants.JOY_DRIVE_EXPO);
y = joyExpo(y, Constants.JOY_DRIVE_EXPO);
theta = joyExpo(theta, Constants.JOY_DRIVE_EXPO);
System.out.print(x);
System.out.print(", ");
System.out.print(y);
System.out.print(", ");
System.out.println(theta);
ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(x, y, theta, Rotation2d.fromDegrees(0));
SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds);
SwerveModuleState lf_optimized = SwerveModuleState.optimize(
moduleStates[0],
Rotation2d.fromDegrees(m_subsystem.lf_turn.getSelectedSensorPosition() / Constants.POD_COUNTS_PER_DEG * 360.0)
);
SwerveModuleState rf_optimized = SwerveModuleState.optimize(
moduleStates[1],
Rotation2d.fromDegrees(m_subsystem.lf_turn.getSelectedSensorPosition() / Constants.POD_COUNTS_PER_DEG * 360.0)
);
SwerveModuleState lb_optimized = SwerveModuleState.optimize(
moduleStates[2],
Rotation2d.fromDegrees(m_subsystem.lf_turn.getSelectedSensorPosition() / Constants.POD_COUNTS_PER_DEG * 360.0)
);
SwerveModuleState rb_optimized = SwerveModuleState.optimize(
moduleStates[3],
Rotation2d.fromDegrees(m_subsystem.lf_turn.getSelectedSensorPosition() / Constants.POD_COUNTS_PER_DEG * 360.0)
);
m_subsystem.lf_turn.set(ControlMode.Position, lf_optimized.angle.getDegrees() / 360.0 * Constants.POD_COUNTS_PER_DEG);
m_subsystem.rf_turn.set(ControlMode.Position, rf_optimized.angle.getDegrees() / 360.0 * Constants.POD_COUNTS_PER_DEG);
m_subsystem.lb_turn.set(ControlMode.Position, lb_optimized.angle.getDegrees() / 360.0 * Constants.POD_COUNTS_PER_DEG);
m_subsystem.rb_turn.set(ControlMode.Position, rb_optimized.angle.getDegrees() / 360.0 * Constants.POD_COUNTS_PER_DEG);
System.out.println(lf_optimized);
m_subsystem.lf_drive.set(ControlMode.PercentOutput, lf_optimized.speedMetersPerSecond / Constants.DRIVE_MAX_SPEED);
m_subsystem.rf_drive.set(ControlMode.PercentOutput, rf_optimized.speedMetersPerSecond / Constants.DRIVE_MAX_SPEED);
m_subsystem.lb_drive.set(ControlMode.PercentOutput, lb_optimized.speedMetersPerSecond / Constants.DRIVE_MAX_SPEED);
m_subsystem.rb_drive.set(ControlMode.PercentOutput, rb_optimized.speedMetersPerSecond / Constants.DRIVE_MAX_SPEED);
}
private double joyExpo(double in, double exp) {
double out;
out = Math.exp(Math.abs(in) * exp) - 1;
out = out / (Math.exp(exp) - 1);
out = Math.signum(in) * out;
return out;
}
// 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;
}
}

21
src/main/java/frc/robot/commands/IdleCommand.java Normal file
View file

@ -0,0 +1,21 @@
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
public class IdleCommand extends CommandBase {
public IdleCommand() {}
@Override
public void initialize() {}
@Override
public void execute() {}
@Override
public void end(boolean interrupted) {}
@Override
public boolean isFinished() {
return false;
}
}

34
src/main/java/frc/robot/commands/SetShear.java Normal file
View file

@ -0,0 +1,34 @@
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.Shear;
public class SetShear extends CommandBase {
private Shear m_shear;
private double m_angle;
public SetShear(Shear shear, double angle) {
addRequirements(shear);
m_shear = shear;
m_angle = angle;
}
@Override
public void initialize() {
m_shear.set(m_angle);
}
@Override
public void execute() {
}
@Override
public void end(boolean interrupted) {
}
@Override
public boolean isFinished() {
return m_shear.atTarget();
}
}

36
src/main/java/frc/robot/commands/SetShooterSpeed.java Normal file
View file

@ -0,0 +1,36 @@
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.PotatoCannon;
public class SetShooterSpeed extends CommandBase {
private PotatoCannon m_cannon;
private double m_speed;
public SetShooterSpeed(PotatoCannon cannon, double speed) {
addRequirements(cannon);
m_cannon = cannon;
m_speed = speed;
// System.out.println("WOT " + speed);
}
@Override
public void initialize() {
// if(m_speed > 0)
// m_cannon.a+=0.1;
// else
// m_cannon.a-=0.1;
m_cannon.set(m_speed);
}
@Override
public void execute() {}
@Override
public void end(boolean interrupted) {}
@Override
public boolean isFinished() {
return true;
}
}

32
src/main/java/frc/robot/commands/ShearGoBrr.java Normal file
View file

@ -0,0 +1,32 @@
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.Shear;
public class ShearGoBrr extends CommandBase {
private Shear m_shear;
public ShearGoBrr(Shear shear) {
addRequirements(shear);
m_shear = shear;
}
@Override
public void initialize() {
m_shear.brr();
}
@Override
public void execute() {
}
@Override
public void end(boolean interrupted) {
m_shear.stop();
}
@Override
public boolean isFinished() {
return false;
}
}

43
src/main/java/frc/robot/commands/Shoot.java Normal file
View file

@ -0,0 +1,43 @@
package frc.robot.commands;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.Gatherer;
import frc.robot.subsystems.PotatoCannon;
public class Shoot extends CommandBase {
private PotatoCannon m_cannon;
private Gatherer m_gatherer;
private double m_startTime;
public Shoot(PotatoCannon cannon, Gatherer gatherer) {
addRequirements(cannon, gatherer);
m_cannon = cannon;
m_gatherer = gatherer;
}
@Override
public void initialize() {
m_startTime = Timer.getFPGATimestamp();
}
@Override
public void execute() {
// if(m_cannon.isAtSpeed()) {
m_gatherer.shoot(false);
if(Timer.getFPGATimestamp() - m_startTime > 1.0) {
m_gatherer.shoot(true);
}
}
@Override
public void end(boolean interrupted) {
m_gatherer.stop();
}
@Override
public boolean isFinished() {
return false;
}
}

31
src/main/java/frc/robot/commands/SnapAngle.java Normal file
View file

@ -0,0 +1,31 @@
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.Chassis;
public class SnapAngle extends CommandBase {
private Chassis m_chassis;
public SnapAngle(Chassis chassis) {
addRequirements(chassis);
m_chassis = chassis;
}
@Override
public void initialize() {
}
@Override
public void execute() {
// if(m_cannon.isAtSpeed()) {
}
@Override
public void end(boolean interrupted) {
}
@Override
public boolean isFinished() {
return false;
}
}

32
src/main/java/frc/robot/commands/Succ.java Normal file
View file

@ -0,0 +1,32 @@
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.Gatherer;
public class Succ extends CommandBase {
private Gatherer m_gatherer;
public Succ(Gatherer gatherer) {
addRequirements(gatherer);
m_gatherer = gatherer;
}
@Override
public void initialize() {
m_gatherer.gather();
}
@Override
public void execute() {
}
@Override
public void end(boolean interrupted) {
m_gatherer.stop();
}
@Override
public boolean isFinished() {
return false;
}
}

35
src/main/java/frc/robot/commands/Wait.java Normal file
View file

@ -0,0 +1,35 @@
package frc.robot.commands;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Wait extends CommandBase {
private double m_startTime;
private double m_time;
public Wait(SubsystemBase subsystem, double time) {
addRequirements(subsystem);
m_time = time;
}
@Override
public void initialize() {
m_startTime = Timer.getFPGATimestamp();
}
@Override
public void execute() {
}
@Override
public void end(boolean interrupted) {
}
@Override
public boolean isFinished() {
double curTime = Timer.getFPGATimestamp();
return curTime - m_startTime > m_time;
}
}

31
src/main/java/frc/robot/commands/ZeroGyro.java Normal file
View file

@ -0,0 +1,31 @@
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.Chassis;
public class ZeroGyro extends CommandBase {
private Chassis m_chassis;
public ZeroGyro(Chassis chassis) {
addRequirements(chassis);
m_chassis = chassis;
}
@Override
public void initialize() {
m_chassis.zeroGyro();
}
@Override
public void execute() {
}
@Override
public void end(boolean interrupted) {
}
@Override
public boolean isFinished() {
return true;
}
}

118
src/main/java/frc/robot/subsystems/Chassis.java Normal file
View file

@ -0,0 +1,118 @@
package frc.robot.subsystems;
import com.kauailabs.navx.frc.AHRS;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.SPI.Port;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import frc.robot.SwervePod;
public class Chassis extends SubsystemBase {
public enum Pod {
LEFT_FRONT,
RIGHT_FRONT,
LEFT_BACK,
RIGHT_BACK
}
private SwerveDriveKinematics m_kinematics;
private SwervePod m_lf_pod = new SwervePod(Constants.LF_TURN, Constants.LF_DRIVE);
private SwervePod m_rf_pod = new SwervePod(Constants.RF_TURN, Constants.RF_DRIVE);
private SwervePod m_lb_pod = new SwervePod(Constants.LB_TURN, Constants.LB_DRIVE);
private SwervePod m_rb_pod = new SwervePod(Constants.RB_TURN, Constants.RB_DRIVE);
public AHRS m_gyro = new AHRS(Port.kMXP);
public Chassis() {
m_kinematics = new SwerveDriveKinematics(
Constants.LF_TRANSLATION,
Constants.RF_TRANSLATION,
Constants.LB_TRANSLATION,
Constants.RB_TRANSLATION);
zeroPodEncoders();
// zeroGyro();
}
public void drive(ChassisSpeeds speeds) {
SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds);
// Optimize pod angles so we arnt doing stupid wrapping
SwerveModuleState lf_optimized = SwerveModuleState.optimize(
moduleStates[0],
Rotation2d.fromDegrees(getPod(Pod.LEFT_FRONT).getAngle())
);
SwerveModuleState rf_optimized = SwerveModuleState.optimize(
moduleStates[1],
Rotation2d.fromDegrees(getPod(Pod.RIGHT_FRONT).getAngle())
);
SwerveModuleState lb_optimized = SwerveModuleState.optimize(
moduleStates[2],
Rotation2d.fromDegrees(getPod(Pod.LEFT_BACK).getAngle())
);
SwerveModuleState rb_optimized = SwerveModuleState.optimize(
moduleStates[3],
Rotation2d.fromDegrees(getPod(Pod.RIGHT_BACK).getAngle())
);
// Set the pod's angle and velocity
getPod(Pod.LEFT_FRONT).set(
lf_optimized.angle.getDegrees(),
lf_optimized.speedMetersPerSecond);
getPod(Pod.RIGHT_FRONT).set(
rf_optimized.angle.getDegrees(),
rf_optimized.speedMetersPerSecond);
getPod(Pod.LEFT_BACK).set(
lb_optimized.angle.getDegrees(),
lb_optimized.speedMetersPerSecond);
getPod(Pod.RIGHT_BACK).set(
rb_optimized.angle.getDegrees(),
rb_optimized.speedMetersPerSecond);
}
public void driveFR(double x, double y, double theta) {
ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
x,
y,
theta,
Rotation2d.fromDegrees(-m_gyro.getAngle()));
drive(speeds);
}
public void zeroPodEncoders() {
m_lf_pod.zeroEncoder();
m_rf_pod.zeroEncoder();
m_lb_pod.zeroEncoder();
m_rb_pod.zeroEncoder();
}
public void zeroGyro() {
m_gyro.zeroYaw();
}
public SwervePod getPod(Pod pod) {
switch(pod) {
case LEFT_FRONT:
return m_lf_pod;
case RIGHT_FRONT:
return m_rf_pod;
case LEFT_BACK:
return m_lb_pod;
case RIGHT_BACK:
return m_rb_pod;
default:
throw new RuntimeException("Invalid pod?");
}
}
@Override
public void periodic() {}
@Override
public void simulationPeriodic() {}
}

59
src/main/java/frc/robot/subsystems/ExampleSubsystem.java
View file

@ -1,59 +0,0 @@
// 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 com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
public class ExampleSubsystem extends SubsystemBase {
public TalonSRX lf_drive = new TalonSRX(Constants.LF_DRIVE);
public TalonSRX lf_turn = new TalonSRX(Constants.LF_TURN);
public TalonSRX rf_drive = new TalonSRX(Constants.RF_DRIVE);
public TalonSRX rf_turn = new TalonSRX(Constants.RF_TURN);
public TalonSRX lb_drive = new TalonSRX(Constants.LB_DRIVE);
public TalonSRX lb_turn = new TalonSRX(Constants.LB_TURN);
public TalonSRX rb_drive = new TalonSRX(Constants.RB_DRIVE);
public TalonSRX rb_turn = new TalonSRX(Constants.RB_TURN);
/** Creates a new ExampleSubsystem. */
public ExampleSubsystem() {
lf_turn.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative);
lf_turn.setSensorPhase(true);
lf_turn.config_kP(0, Constants.TURN_P);
lf_turn.config_kI(0, Constants.TURN_I);
lf_turn.config_kD(0, Constants.TURN_D);
rf_turn.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative);
rf_turn.setSensorPhase(true);
rf_turn.config_kP(0, Constants.TURN_P);
rf_turn.config_kI(0, Constants.TURN_I);
rf_turn.config_kD(0, Constants.TURN_D);
lb_turn.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative);
lb_turn.setSensorPhase(true);
lb_turn.config_kP(0, Constants.TURN_P);
lb_turn.config_kI(0, Constants.TURN_I);
lb_turn.config_kD(0, Constants.TURN_D);
rb_turn.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative);
rb_turn.setSensorPhase(true);
rb_turn.config_kP(0, Constants.TURN_P);
rb_turn.config_kI(0, Constants.TURN_I);
rb_turn.config_kD(0, Constants.TURN_D);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
@Override
public void simulationPeriodic() {
// This method will be called once per scheduler run during simulation
}
}

194
src/main/java/frc/robot/subsystems/Gatherer.java Normal file
View file

@ -0,0 +1,194 @@
package frc.robot.subsystems;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
public class Gatherer extends SubsystemBase {
private enum GatherState {
IDLE,
WAITING
}
private enum ManageState {
EMPTY,
ENTERING_LOW,
TRANSFERRING,
LEAVING_HIGH
}
private enum BallState {
NO_BALLS,
ENTERING_LOW,
LOW,
TRANSFERRING,
HIGH,
TWO_BALLS,
SATURATED
}
private TalonSRX m_gather_low = new TalonSRX(Constants.GATHER_LOW);
private TalonSRX m_gather_high_back = new TalonSRX(Constants.GATHER_HIGH_BACK);
private TalonSRX m_gather_high_front = new TalonSRX(Constants.GATHER_HIGH_FRONT);
private DigitalInput m_lowSensor = new DigitalInput(Constants.GATHER_LOW_DIO);
private DigitalInput m_highSensor = new DigitalInput(Constants.GATHER_HIGH_DIO);
private int m_ballsInTransport = 0;
private boolean m_prevLowSensor = false;
private boolean m_prevHighSensor = false;
private boolean m_haveLowBall = false;
private boolean m_haveHighBall = false;
private BallState m_BallState = BallState.NO_BALLS;
private GatherState m_state = GatherState.IDLE;
private ManageState m_manageState = ManageState.EMPTY;
public double shoot_speed_front = 1.0;
public double shoot_speed_back = 0.5;
public Gatherer() {
m_gather_low.setInverted(false);
m_gather_high_back.setInverted(true);
m_gather_high_front.setInverted(false);
}
public boolean haveOneBall() {
return !m_lowSensor.get();
}
public boolean haveTwoBall() {
return false;
}
public void gather() {
// m_state = GatherState.GATHERING;
setGather(1);
// setTower(0.6);
m_gather_high_back.set(ControlMode.PercentOutput, -0.6);
m_gather_high_front.set(ControlMode.PercentOutput, 0.6);
}
public void store() {
setGather(1);
m_gather_high_back.set(ControlMode.PercentOutput, shoot_speed_back);
m_gather_high_front.set(ControlMode.PercentOutput, shoot_speed_front);
}
public void eject() {
// m_state = GatherState.EJECTING;
setGather(-1);
setTower(-0.6);
}
public void shoot(boolean full) {
// m_state = GatherState.SHOOTING;
// setTower(1);
if(full) {
setGather(1.0);
}
m_gather_high_back.set(ControlMode.PercentOutput, shoot_speed_back);
m_gather_high_front.set(ControlMode.PercentOutput, shoot_speed_front);
}
public void stop() {
m_state = GatherState.IDLE;
setGather(0);
setTower(0);
}
public void setGather(double speed) {
m_gather_low.set(ControlMode.PercentOutput, speed);
}
public void setTower(double speed) {
m_gather_high_back.set(ControlMode.PercentOutput, speed);
m_gather_high_front.set(ControlMode.PercentOutput, speed);
}
@Override
public void periodic() {
boolean lowSensor = m_lowSensor.get();
boolean highSensor = m_highSensor.get();
boolean lowStateChanged = false;
boolean highStateChanged = false;
if(m_prevLowSensor != lowSensor) {
lowStateChanged = true;
m_prevLowSensor = lowSensor;
}
if(m_prevHighSensor != highSensor) {
highStateChanged = true;
m_prevHighSensor = highSensor;
}
switch(m_BallState) {
case NO_BALLS:
break;
case ENTERING_LOW:
// setGather(1);
break;
case LOW:
break;
case TRANSFERRING:
break;
case HIGH:
break;
case TWO_BALLS:
break;
case SATURATED:
break;
}
// switch(m_state) {
// case IDLE:
// setGather(0);
// setTower(0);
// break;
// case GATHERING:
// setGather(1);
// break;
// case EJECTING:
// setGather(-1);
// break;
// case SHOOTING:
// setTower(1);
// break;
// case MANAGING:
// manage();
// break;
// }
}
private void manage() {
switch(m_manageState) {
case EMPTY:
break;
case ENTERING_LOW:
setGather(1);
// if(lowStateChanged && lowSensor) {
// setGather(0);
// m_haveLowBall = true;
// }
break;
case TRANSFERRING:
break;
case LEAVING_HIGH:
break;
}
}
@Override
public void simulationPeriodic() {
// This method will be called once per scheduler run during simulation
}
}

42
src/main/java/frc/robot/subsystems/PotatoCannon.java Normal file
View file

@ -0,0 +1,42 @@
package frc.robot.subsystems;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
public class PotatoCannon extends SubsystemBase {
public TalonSRX m_shooter = new TalonSRX(Constants.SHOOTER);
public double a = 0.0;
public PotatoCannon() {
m_shooter.setInverted(false);
// m_shooter.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative);
// m_shooter.setSensorPhase(true);
// m_shooter.config_kP(0, Constants.TURN_P);
// m_shooter.config_kI(0, Constants.TURN_I);
// m_shooter.config_kD(0, Constants.TURN_D);
}
public void set(double rpm) {
// m_shooter.set(ControlMode.Velocity, rpm * Constants.SHOOTER_COUNTS_PER_REV);
m_shooter.set(ControlMode.PercentOutput, rpm);
}
public boolean isAtSpeed() {
return Math.abs(m_shooter.getClosedLoopError()) < Constants.SHOOTER_TARGET_ERROR;
}
@Override
public void periodic() {
// m_shooter.set(ControlMode.PercentOutput, 0.1);
// This method will be called once per scheduler run
}
@Override
public void simulationPeriodic() {
// This method will be called once per scheduler run during simulation
}
}

60
src/main/java/frc/robot/subsystems/Shear.java Normal file
View file

@ -0,0 +1,60 @@
package frc.robot.subsystems;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
public class Shear extends SubsystemBase {
public TalonSRX m_left = new TalonSRX(Constants.SHEAR_LEFT);
public TalonSRX m_right = new TalonSRX(Constants.SHEAR_RIGHT);
public Shear() {
m_left.setInverted(true);
// m_right.setInverted(true);
m_left.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative);
// m_left.setSensorPhase(true);
m_left.config_kP(0, Constants.SHEAR_P);
m_left.config_kI(0, Constants.SHEAR_I);
m_left.config_kD(0, Constants.SHEAR_D);
m_right.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative);
// m_right.setSensorPhase(true);
m_right.config_kP(0, Constants.SHEAR_P);
m_right.config_kI(0, Constants.SHEAR_I);
m_right.config_kD(0, Constants.SHEAR_D);
}
public void set(double angle) {
// m_shooter.set(ControlMode.Velocity, rpm * Constants.SHOOTER_COUNTS_PER_REV);
m_left.set(ControlMode.Position, angle);
m_right.set(ControlMode.Position, angle);
}
public boolean atTarget() {
return (m_left.getClosedLoopError() < Constants.SHEAR_TOLERANCE) && (m_right.getClosedLoopError() < Constants.SHEAR_TOLERANCE);
}
public void brr() {
m_left.set(ControlMode.PercentOutput, 1.0);
m_right.set(ControlMode.PercentOutput, 1.0);
}
public void stop() {
m_left.set(ControlMode.PercentOutput, 0.0);
m_right.set(ControlMode.PercentOutput, 0.0);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
@Override
public void simulationPeriodic() {
// This method will be called once per scheduler run during simulation
}
}

35
vendordeps/navx_frc.json Normal file
View file

@ -0,0 +1,35 @@
{
"fileName": "navx_frc.json",
"name": "KauaiLabs_navX_FRC",
"version": "4.0.442",
"uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
"mavenUrls": [
"https://repo1.maven.org/maven2/"
],
"jsonUrl": "https://www.kauailabs.com/dist/frc/2022/navx_frc.json",
"javaDependencies": [
{
"groupId": "com.kauailabs.navx.frc",
"artifactId": "navx-java",
"version": "4.0.442"
}
],
"jniDependencies": [],
"cppDependencies": [
{
"groupId": "com.kauailabs.navx.frc",
"artifactId": "navx-cpp",
"version": "4.0.442",
"headerClassifier": "headers",
"sourcesClassifier": "sources",
"sharedLibrary": false,
"libName": "navx_frc",
"skipInvalidPlatforms": true,
"binaryPlatforms": [
"linuxathena",
"linuxraspbian",
"windowsx86-64"
]
}
]
}