diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..5f9d275 --- /dev/null +++ b/simgui.json @@ -0,0 +1,10 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo" + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c50ba05..3f498b8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,7 +13,58 @@ * constants are needed, to reduce verbosity. */ public final class Constants { + public static class Ports { + public static int kDriveFrontLeftMove = 1; + public static int kDriveFrontLeftTurn = 2; + public static int kDriveFrontRightMove = 3; + public static int kDriveFrontRightTurn = 4; + public static int kDriveBackLeftMove = 5; + public static int kDriveBackLeftTurn = 6; + public static int kDriveBackRightMove = 7; + public static int kDriveBackRightTurn = 8; + } + + public static class MotorConstants{ + public static double distancePerRotation=10.0; + public static double movementPerRotation=10.0; + } + public static class OperatorConstants { public static final int kDriverControllerPort = 0; + public static final int snapButton=1; + public static final int AmpShotButton=2; + } + + public static class PIDConstants { + public static final double kDrivetrainP = 0.2; + public static final double kDrivetrainI = 0.0; + public static final double kDrivetrainD = 0.0; + public static final double kRobotTurnP = 0.2; + public static final double kRobotTurnI = 0.0; + public static final double kRobotTurnD = 0.0; + public static final double kRobotMoveP = 0.2; + public static final double kRobotMoveI = 0.0; + public static final double kRobotMoveD = 0.0; + } + public static class AprilTags{ + //this stores the settings for the april tag detector config and the families to be detected by the detector + //remember to actually configure these + public static final int numThreads=1; + public static final float quadDecimate=2; + public static final float quadSigma=0; + //6.5 inches to mm + public static final double normalTagLength=165.1; + //the number of pixels the border of the april tag will have at 1m. + public static final double pixelsAtMeter=400; + //the X coordinate of the center when the april tag is aligned with the camera. + public static final double centerXCoordinate=300; + //https://docs.wpilib.org/en/stable/docs/software/vision-processing/apriltag/apriltag-intro.html + public static final String family="36h11"; + } + public static class AmpAprilTag{ + //remember to actually fill this in with the correct april tags + public static final int ampNum=0; + //the optimal distance at which the note can be inserted into the amp in m + public static final double optimalDistance=0.1; } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 687a0a0..a0fc394 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,7 +4,9 @@ package frc.robot; +import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.motorcontrol.Talon; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -18,6 +20,11 @@ public class Robot extends TimedRobot { private Command m_autonomousCommand; private RobotContainer m_robotContainer; + private Talon FrontLeftMove; + private Talon FrontRightMove; + private Talon BackLeftMove; + private Talon BackRightMove; + private Encoder frontLeftMove; /** * This function is run when the robot is first started up and should be used for any @@ -87,11 +94,28 @@ public void teleopPeriodic() {} public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); + FrontLeftMove= new Talon(0); + FrontRightMove= new Talon(0); + BackLeftMove= new Talon(0); + BackRightMove= new Talon(0); + frontLeftMove = new Encoder(0, 0); + FrontLeftMove.set(0.1); + FrontRightMove.set(0.1); + BackLeftMove.set(0.1); + BackRightMove.set(0.1); } + /** This function is called periodically during test mode. */ @Override - public void testPeriodic() {} + public void testPeriodic() { + if (frontLeftMove.get()==100){ + FrontLeftMove.set(0); + FrontRightMove.set(0); + BackLeftMove.set(0); + BackRightMove.set(0); + } + } /** This function is called once when the robot is first started up. */ @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a33249e..553c257 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,13 +4,37 @@ package frc.robot; -import frc.robot.Constants.OperatorConstants; +import frc.robot.commands.AlignWithAprilTag; +// here is where you put all your commands and subsystems; import frc.robot.commands.Autos; -import frc.robot.commands.ExampleCommand; +import frc.robot.commands.Drive; +import frc.robot.commands.Snap; +import frc.robot.commands.AlignWithAprilTag; +import frc.robot.commands.MoveToAmp; import frc.robot.subsystems.ExampleSubsystem; +import frc.robot.subsystems.Encoders; +import frc.robot.subsystems.Motors; +import frc.robot.subsystems.Gyroscope; +import frc.robot.subsystems.TagDetector; +import frc.robot.Constants.Ports; +import frc.robot.Constants.OperatorConstants; +import frc.robot.Constants.MotorConstants; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.apriltag.AprilTagDetector; +import edu.wpi.first.apriltag.AprilTagDetector.Config; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.SparkAbsoluteEncoder.Type; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.kauailabs.navx.frc.AHRS; +import edu.wpi.first.wpilibj.I2C; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -21,15 +45,52 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); + // the video says to define the motors within the motors subsystem, but I will just define it here so I know where all my + // variables are + + // remember to configure the acutal channels + private final CANSparkMax frontLeftMove= new CANSparkMax(Ports.kDriveFrontLeftMove,MotorType.kBrushless); + private final CANSparkMax frontLeftTurn= new CANSparkMax(Ports.kDriveFrontLeftTurn,MotorType.kBrushless); + private final CANSparkMax frontRightMove= new CANSparkMax(Ports.kDriveFrontRightMove,MotorType.kBrushless); + private final CANSparkMax frontRightTurn= new CANSparkMax(Ports.kDriveFrontRightTurn,MotorType.kBrushless); + private final CANSparkMax backLeftMove= new CANSparkMax(Ports.kDriveBackLeftMove,MotorType.kBrushless); + private final CANSparkMax backLeftTurn = new CANSparkMax(Ports.kDriveBackLeftTurn,MotorType.kBrushless); + private final CANSparkMax backRightMove= new CANSparkMax(Ports.kDriveBackRightMove,MotorType.kBrushless); + private final CANSparkMax backRightTurn= new CANSparkMax(Ports.kDriveBackRightTurn,MotorType.kBrushless); + private final AbsoluteEncoder frontLeftMoveEncoder = frontLeftMove.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder frontLeftTurnEncoder = frontLeftTurn.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder frontRightMoveEncoder = frontRightMove.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder frontRightTurnEncoder = frontRightTurn.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder backLeftMoveEncoder = backLeftMove.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder backLeftTurnEncoder = backLeftTurn.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder backRightMoveEncoder = backRightMove.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder backRightTurnEncoder = backRightTurn.getAbsoluteEncoder(Type.kDutyCycle); + private final AHRS gyro = new AHRS(I2C.Port.kMXP); + private final AprilTagDetector detector= new AprilTagDetector(); + private final AprilTagDetector.Config configuration= new Config(); + + + private final Motors motors= new Motors(frontLeftMove, frontLeftTurn, frontRightMove, frontRightTurn, backLeftMove, backLeftTurn, backRightMove, backRightTurn); + //motor radius is configured in mm and distance per rotation is still unkown + private final Encoders encoders= new Encoders(frontLeftMoveEncoder, frontLeftTurnEncoder, frontRightTurnEncoder, frontRightMoveEncoder, backLeftMoveEncoder, backLeftTurnEncoder, backRightMoveEncoder, backRightTurnEncoder, MotorConstants.movementPerRotation); + private final Gyroscope gyroscope = new Gyroscope(gyro); + private final TagDetector tagDetector= new TagDetector(detector, configuration); + + // remember to set the joystick port - // Replace with CommandPS4Controller or CommandJoystick if needed - private final CommandXboxController m_driverController = - new CommandXboxController(OperatorConstants.kDriverControllerPort); + private Joystick stick = new Joystick(OperatorConstants.kDriverControllerPort); + // this is the button on the handle of the joystick + private JoystickButton snapButton = new JoystickButton(stick, OperatorConstants.snapButton); + private JoystickButton ampShotButton= new JoystickButton(stick, OperatorConstants.AmpShotButton); + + //remember to add the actual shoot into amp command in here. + private final SequentialCommandGroup autoAmpShot= new SequentialCommandGroup(new AlignWithAprilTag(encoders, motors, tagDetector), new MoveToAmp(encoders, motors, tagDetector)); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Configure the trigger bindings configureBindings(); + defaultCommands(); } /** @@ -42,15 +103,13 @@ public RobotContainer() { * joysticks}. */ private void configureBindings() { - // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - new Trigger(m_exampleSubsystem::exampleCondition) - .onTrue(new ExampleCommand(m_exampleSubsystem)); - - // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, - // cancelling on release. - m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); + snapButton.whileTrue(new Snap(motors, encoders, gyroscope, stick)); + ampShotButton.whileTrue(autoAmpShot); + + } + private void defaultCommands(){ + motors.setDefaultCommand(new Drive(motors,encoders,gyroscope,stick)); } - /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/frc/robot/commands/AlignWithAprilTag.java b/src/main/java/frc/robot/commands/AlignWithAprilTag.java new file mode 100644 index 0000000..8155b34 --- /dev/null +++ b/src/main/java/frc/robot/commands/AlignWithAprilTag.java @@ -0,0 +1,217 @@ +// 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.subsystems.Encoders; +import frc.robot.subsystems.Encoders.TurnEncoder; +import frc.robot.subsystems.Motors; +import frc.robot.subsystems.Motors.TurnMotor; +import frc.robot.subsystems.TagDetector; +import frc.robot.Constants.AmpAprilTag; +import frc.robot.Constants.PIDConstants; + +import javax.swing.text.StyleContext.SmallAttributeSet; + +import org.opencv.core.Mat; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +/** An example command that uses an example subsystem. */ +public class AlignWithAprilTag extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final Encoders encoders; + private final Motors motors; + private final TagDetector tagDetector; + + private PIDController bearingControllerFrontLeft; + private PIDController bearingControllerFrontRight; + private PIDController bearingControllerBackLeft; + private PIDController bearingControllerBackRight; + private PIDController robotBearingController; + + public ShuffleboardTab tab; + public GenericEntry P; + public GenericEntry I; + public GenericEntry D; + //for the motors + public double kP; + public double kI; + public double kD; + //for the robot + public double kPRobotTurn; + public double kIRobotTurn; + public double kDRobotTurn; + + public int tag; + public Mat image; + public double angleDiff; + + public boolean firstIteration; + public boolean foundTurningDirection; + public double previousAngleDiff; + public boolean inverseDirection; + public double orientationTime; + public boolean orientationEnter; + + public boolean endCommand; + public double currentBearing; + + + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public AlignWithAprilTag(Encoders encoders, Motors motors, TagDetector tagDetector) { + //remember to add the camera to this command. + + this.encoders=encoders; + this.motors=motors; + this.tagDetector=tagDetector; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(encoders,motors,tagDetector); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + kP=PIDConstants.kDrivetrainP; + kI=PIDConstants.kDrivetrainI; + kD=PIDConstants.kDrivetrainD; + + + kPRobotTurn=PIDConstants.kRobotTurnP; + kIRobotTurn=PIDConstants.kRobotTurnI; + kDRobotTurn=PIDConstants.kRobotTurnD; + + //the setpoint is set in a way that will cause the robot to enter turning mode, + //but it does not necessarily have to be used. + bearingControllerFrontLeft=new PIDController(kP, kI, kD); + bearingControllerFrontLeft.enableContinuousInput(0, 2*Math.PI); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerFrontLeft.setTolerance(Math.PI/360); + bearingControllerFrontLeft.setSetpoint(Math.PI/4); + + bearingControllerFrontRight=new PIDController(kP, kI, kD); + bearingControllerFrontRight.enableContinuousInput(0, 2*Math.PI); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerFrontRight.setTolerance(Math.PI/360); + bearingControllerFrontRight.setSetpoint(Math.PI/4*3); + + bearingControllerBackLeft=new PIDController(kP, kI, kD); + bearingControllerBackLeft.enableContinuousInput(0, 2*Math.PI); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerBackLeft.setTolerance(Math.PI/360); + bearingControllerBackLeft.setSetpoint(Math.PI/4*5); + + bearingControllerBackRight=new PIDController(kP, kI, kD); + bearingControllerBackRight.enableContinuousInput(0, 2*Math.PI); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerBackRight.setTolerance(Math.PI/360); + bearingControllerBackRight.setSetpoint(Math.PI/4*7);; + + //the PID controller for the robot itself after turning mode has been achieved + robotBearingController= new PIDController(kPRobotTurn, kIRobotTurn, kDRobotTurn); + robotBearingController.enableContinuousInput(0, 2*Math.PI); + robotBearingController.setTolerance(Math.PI/360); + + + tag=AmpAprilTag.ampNum; + //giving it a dummy value + angleDiff=10; + firstIteration=true; + orientationEnter=true; + SmartDashboard.putBoolean("Entered align Command", true); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + //take a picture + if (tagDetector.updateDetection(image, tag)==false){ + //essntially, if an image is not found, stop this command + tab=Shuffleboard.getTab("AmpShot Command"); + tab.add("Error","Image not found"); + } + angleDiff=tagDetector.angleDiff(tagDetector.distanceToTag()); + if (firstIteration){ + previousAngleDiff=angleDiff; + firstIteration=false; + } + if (angleDiff>Math.PI/360){ + + //allowing an error of 1 degrees. + if(bearingControllerFrontLeft.atSetpoint() || + bearingControllerFrontRight.atSetpoint() || + bearingControllerBackLeft.atSetpoint() || + bearingControllerBackRight.atSetpoint()){ + + if(foundTurningDirection){ + robotBearingController.setSetpoint(0); + if (inverseDirection){ + motors.setMoveMotors(-1*robotBearingController.calculate(angleDiff)); + } else { + motors.setMoveMotors(robotBearingController.calculate(angleDiff)); + } + } + else{ + //turn right for 0.5s to check if the robot should turn anticlockwise or clockwise. + if (orientationEnter){ + orientationTime=System.currentTimeMillis(); + orientationEnter=false; + } + motors.setMoveMotors(0.5); + if (System.currentTimeMillis()-orientationTime>500){ + //essentially, if this is a second iteration, then check how setting move motors affected + //the angle difference + if (previousAngleDiff