Skip to content

Vision #7

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Lori2018 wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Vision #7

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
ff31fc7
Vision
Lori2018 Mar 10, 2021
09ac3ef
update
Lori2018 Mar 20, 2021
2523ddc
sketchy but works
Lori2018 Jun 17, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions RobotCode2021-Imported/build.gradle
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,10 @@ dependencies {
simulation wpi.deps.sim.gui(wpi.platforms.desktop, false)
simulation wpi.deps.sim.driverstation(wpi.platforms.desktop, false)

compile 'com.googlecode.json-simple:json-simple:1.1.1'



// Websocket extensions require additional configuration.
// simulation wpi.deps.sim.ws_server(wpi.platforms.desktop, false)
// simulation wpi.deps.sim.ws_client(wpi.platforms.desktop, false)
Expand Down
6 changes: 3 additions & 3 deletions RobotCode2021-Imported/src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -196,9 +196,9 @@ public static final class Yellow {
}

public static final class Vision {
public static final double kCameraHeight = 0.23; // meter -- REMEASURE
public static final double kCameraPitch = 0.436; // radians
public static final double kTargetHeight = 2.06; // meters
public static final double kCameraHeight = 0.5461; // meter -- REMEASURE
public static final double kCameraPitch = 1.249; // radians
public static final double kTargetHeight = 2.49555 / 2; // meters

public static final double turretKP = 0;
public static final double turretKI = 0;
Expand Down
6 changes: 6 additions & 0 deletions RobotCode2021-Imported/src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,7 @@ public void robotInit() {
@Override
public void robotPeriodic() {
Scheduler.getInstance().run();
SmartDashboard.putString("SCHEDULER STATUS", Scheduler.getInstance().getClass().toString());
// SmartDashboard.putNumber("hoodEncoder", hoodEncoder.getPosition());
// SmartDashboard.putNumber("hoodVoltage", hoodEncoder.getBusVoltage());
}
Expand All @@ -69,6 +70,7 @@ public void disabledPeriodic() {

@Override
public void autonomousInit() {
<<<<<<< Updated upstream
RobotContainer.swerveDrive.resetOdometry(new Pose2d(0, 0, new Rotation2d(-Math.PI / 2.)));
RobotContainer.theta.reset(-Math.PI / 2.);
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
Expand All @@ -77,6 +79,10 @@ public void autonomousInit() {
if (m_autonomousCommand != null) {
Scheduler.getInstance().add(m_autonomousCommand);
}
=======
RobotContainer.swerveDrive.resetOdometry(new Pose2d(0, 0, new Rotation2d(Math.PI / 2))); //0.053, .8539
RobotContainer.theta.reset(Math.PI / 2);
>>>>>>> Stashed changes

try {
f = new File("/home/lvuser/Output.txt");
Expand Down
188 changes: 69 additions & 119 deletions RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,8 @@

import java.util.List;

import org.photonvision.PhotonCamera;

import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
Expand Down Expand Up @@ -49,14 +51,13 @@ public class RobotContainer {
private final Conveyor conveyor = new Conveyor();
public final static Shooter shooter = new Shooter();
private final Climb climb = new Climb();
public final Vision vision = new Vision();
private final Turret turret = new Turret(vision, swerveDrive);

private final PhotonCamera camera = new PhotonCamera("TurretCamera");
private final Vision vision = new Vision(camera);
private final Turret turret = new Turret();
// The driver's controller
public static Joystick m_driverController = new Joystick(OIConstants.kDriverControllerPort);
public static Joystick m_operatorController = new Joystick(1);

// public static final GenericHID.RumbleType kLeftRumble = 1;


public static ProfiledPIDController theta = new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0,
Expand All @@ -70,38 +71,17 @@ public class RobotContainer {
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
// Gets the default instance of NetworkTables
NetworkTableInstance table = NetworkTableInstance.getDefault();

// Gets the MyCamName table under the chamelon-vision table
// MyCamName will vary depending on the name of your camera
NetworkTable cameraTable = table.getTable("chameleon-vision").getSubTable("Shooter");

// Gets the yaw to the target from the cameraTable
yaw = cameraTable.getEntry("yaw");

// Gets the driveMode boolean from the cameraTable
isDriverMode = cameraTable.getEntry("driver_mode");


// Configure the button bindings
configureButtonBindings();

// Configure default commands
// Set the default drive command to split-stick arcade drive
swerveDrive.setDefaultCommand(new DefaultDrive(swerveDrive, m_driverController, false));
swerveDrive.setDefaultCommand(new DefaultDrive(swerveDrive, vision, m_driverController, 1));
conveyor.setDefaultCommand(new AutoIndexConveyor(conveyor));
intake.setDefaultCommand(new RunIntake(intake, m_operatorController));
turret.setDefaultCommand(new SpinTurret(turret, false, 0));

// vision.setDefaultCommand(new RunCommand(vision::runVision, vision));

// swerveDrive.setDefaultCommand(
turret.setDefaultCommand(new SpinTurret(turret, swerveDrive, vision, 2, 0));

// new InstantCommand(() -> swerveDrive.drive(-m_driverController.getRawAxis(1)
// * Constants.SwerveDriveConstants.kMaxSpeedMetersPerSecond,
// -m_driverController.getRawAxis(0)
// * Constants.SwerveDriveConstants.kMaxSpeedMetersPerSecond,
// -m_driverController.getRawAxis(4) * (2 * Math.PI), true), swerveDrive));
}

/**
Expand All @@ -114,15 +94,48 @@ public RobotContainer() {
private void configureButtonBindings() {
JoystickButton butA = new JoystickButton(m_operatorController, 1);
JoystickButton butB = new JoystickButton(m_operatorController, 2);
JoystickButton butY = new JoystickButton(m_operatorController, 3);
JoystickButton butpress = new JoystickButton(m_driverController, 8);
JoystickButton butXd = new JoystickButton(m_driverController, 8);
JoystickButton butX = new JoystickButton(m_operatorController, 3);

JoystickButton butXd = new JoystickButton(m_driverController, 3);
JoystickButton butAd = new JoystickButton(m_driverController, 1);
JoystickButton butBd = new JoystickButton(m_driverController, 2);

JoystickButton gyro = new JoystickButton(m_driverController, 8);

JoystickButton rBump = new JoystickButton(m_operatorController, 6);
JoystickButton lBump = new JoystickButton(m_operatorController, 5);
JoystickButton lAnal = new JoystickButton(m_operatorController, 9);
JoystickButton rAnal = new JoystickButton(m_operatorController, 10);
JoystickButton gyro = new JoystickButton(m_driverController, 7);

// B
JoystickButton turbo = new JoystickButton(m_driverController, 2);
// JoystickButton ok = new JoystickButton(m_driverController, 7);

BarrelPath Barrel = new BarrelPath(swerveDrive, theta);

SmartDashboard.putData(Scheduler.getInstance());
SmartDashboard.putData("Barrel Path", Barrel);

// GalacticA galA = new GalacticA(swerveDrive, theta);

SmartDashboard.putData(Scheduler.getInstance());
// SmartDashboard.putData("GalacticA", galA);


BouncePath Bounce = new BouncePath(swerveDrive, theta);

SmartDashboard.putData(Scheduler.getInstance());
SmartDashboard.putData("Bounce Path", Bounce);


SlalomPath1 Slalom = new SlalomPath1(swerveDrive, theta);

SmartDashboard.putData(Scheduler.getInstance());
SmartDashboard.putData("Slalom Path", Slalom);

SideStep SideStep = new SideStep(swerveDrive, theta);
butAd.whenPressed(SideStep);


// A button
butA.whileHeld(new ExtendIntake(intake, m_operatorController));
Expand All @@ -133,40 +146,39 @@ private void configureButtonBindings() {
// right bumper
rBump.whileHeld(new RunShooter(shooter));
rBump.whenReleased(new StopShooter(shooter));

rBump.whileHeld(new FeedShooter(conveyor, shooter));
rBump.whenReleased(new AutoIndexConveyor(conveyor));

// left bumper
lBump.whileHeld(new ControlConveyor(conveyor));
lBump.whenReleased(new AutoIndexConveyor(conveyor));

// left analog center
lAnal.whileHeld(new MoveHood(shooter, -1));

lAnal.whileHeld(new MoveHood(shooter, -1));

// right analog center
rAnal.whileHeld(new MoveHood(shooter, 1));

// right bumper
rBump.whileHeld(new FeedShooter(conveyor, shooter));
rBump.whenReleased(new AutoIndexConveyor(conveyor));

// left bumper
lBump.whileHeld(new ControlConveyor(conveyor));
lBump.whenReleased(new AutoIndexConveyor(conveyor));


// B button
butB.whileHeld(new SpinTurret(turret, true, 0.25));
butB.whenReleased(new SpinTurret(turret, true, 0));
butB.whileHeld(new SpinTurret(turret, swerveDrive, vision, 1, 0.25));

// Y button
butY.whileHeld(new SpinTurret(turret, true, -0.25));
butY.whenReleased(new SpinTurret(turret, true, 0));
butX.whileHeld(new SpinTurret(turret, swerveDrive, vision, 1, -0.25));

// driver X button
butXd.whileHeld(new DefaultDrive(swerveDrive, m_driverController, true));
// driver X button - slow
butXd.whileHeld(new DefaultDrive(swerveDrive, vision, m_driverController, 0.35));
butAd.whenPressed(new InstantCommand(swerveDrive::zeroWheels));

gyro.whenPressed(new InstantCommand(swerveDrive::zeroHeading));
butBd.whenPressed(new SideStep(swerveDrive, theta));

//new JoystickButton(m_operatorController, 4).whenPressed(new RunCommand(() -> conveyor.manualControl(-), conveyor))
// .whenReleased(new RunCommand(conveyor::autoIndex, conveyor));
// should be start button for camera to find target idk what number is so fix it
// new JoystickButton(m_operatorController, 7).whenHeld(new InstantCommand(turret::visionTurret, turret));

turbo.whileHeld(new DefaultDrive(swerveDrive, vision, m_driverController, 2));

gyro.whenPressed(new InstantCommand(swerveDrive::zeroHeading));

}

public static String getCoords() {
Expand All @@ -178,74 +190,12 @@ public static String getCoords() {
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// Create config for trajectory
TrajectoryConfig config = new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond,
AutoConstants.kMaxAccelerationMetersPerSecondSquared)
// Add kinematics to ensure max speed is actually obeyed
.setKinematics(SwerveDriveConstants.kDriveKinematics);

// An example trajectory to follow. All units in meters.
// Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
// Start at the origin facing the +X direction
// new Pose2d(0, 0, new Rotation2d(0)),
// List.of(),
// End 3 meters straight ahead of where we started, facing forward
// new Pose2d(0, 3, new Rotation2d(0)), config);

Trajectory newTrajectory = TrajectoryGenerator.generateTrajectory(
new Pose2d(0, 0, new Rotation2d(-Math.PI / 2)), List.of(
// keep in
new Translation2d(0, -1.2), // take out
new Translation2d(1.1, -1.9),
new Translation2d(1.88, -1.9),


//new Translation2d(1.88, -3.5),
//new Translation2d(1.88, -4),
// keep in
new Translation2d(1.88, -5.5),
new Translation2d(1.88, -6.25),
new Translation2d(0.3, -6.31),
new Translation2d(0.3, -7.9)


// previously commented out
//new Translation2d(1.324, -7.9),
//new Translation2d(1.724, -7.9),
// new Translation2d(1.6, -7.75),
// new Translation2d(1.6, -6.55),
// new Translation2d(0, -6.3)
//new Translation2d(1.78, -7.75)
),

new Pose2d(1.88, -7.75, new Rotation2d(-Math.PI / 2)), config);
// new Pose2d(0, -7, new Rotation2d(-Math.PI / 2)), config);



SwerveControllerCommand swerveControllerCommand1 = new SwerveControllerCommand(Slalom.getTrajectory(),
(-Math.PI / 2), swerveDrive::getPose, // Functional interface to feed supplier
SwerveDriveConstants.kDriveKinematics,

// Position controllers
new PIDController(AutoConstants.kPXController, 0, 0),
new PIDController(AutoConstants.kPYController, 0, 0), theta,

swerveDrive::setModuleStates,

swerveDrive

);

// Command shootCommand = new Command(() -> shooter.runHood(.5), shooter)
// .andThen(shooter::runShooter, shooter)
// .andThen(new RunCommand(() -> conveyor.feedShooter(0.75, shooter.atSpeed()), conveyor))
// .withTimeout(15).andThen(new InstantCommand(shooter::stopShooter, shooter));



public Command getAutonomousCommand(Trajectory trajectory) {
// GalacticA galA = new GalacticA(swerveDrive, theta);
BarrelPath bar = new BarrelPath(swerveDrive, theta);

// return swerveControllerCommand1;
return bar;

return swerveControllerCommand1;
}
Expand Down
68 changes: 68 additions & 0 deletions RobotCode2021-Imported/src/main/java/frc/robot/commands/Auto/GalacticA.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,68 @@
// // 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.Auto;

// import edu.wpi.first.wpilibj.command.Command;
// import edu.wpi.first.wpilibj.command.CommandGroup;
// import edu.wpi.first.wpilibj.command.InstantCommand;

// import java.util.List;

// import edu.wpi.first.wpilibj.geometry.Pose2d;
// import edu.wpi.first.wpilibj.geometry.Rotation2d;
// import edu.wpi.first.wpilibj.geometry.Translation2d;
// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
// import edu.wpi.first.wpilibj.trajectory.Trajectory;
// import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
// import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
// import frc.robot.commands.*;
// import edu.wpi.first.wpilibj.controller.PIDController;
// import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
// import frc.robot.RobotContainer;
// import frc.robot.Constants.AutoConstants;
// import frc.robot.Constants.SwerveDriveConstants;
// import frc.robot.subsystems.*;

// public class GalacticA extends CommandGroup {
// /** Add your docs here. */
// public GalacticA(SwerveDrive swerveDrive, ProfiledPIDController theta) {

// final RobotContainer m_robotContainer;
// final Intake intake;

// // Create config for trajectory
// TrajectoryConfig config = new TrajectoryConfig(2.1,
// AutoConstants.kMaxAccelerationMetersPerSecondSquared)
// // Add kinematics to ensure max speed is actually obeyed
// // .setKinematics(SwerveDriveConstants.kDriveKinematics)
// .setEndVelocity(1.5);

// Trajectory trajectory = TrajectoryGenerator
// .generateTrajectory(new Pose2d(0, 0, new Rotation2d(-Math.PI / 2)), List.of(

// ),
// // direction robot moves
// new Pose2d(0, -3, new Rotation2d(Math.PI / 2)), config);

// SwerveControllerCommand swerveControllerCommand1 = new SwerveControllerCommand(trajectory, (0),
// swerveDrive::getPose, // Functional interface to feed supplier
// SwerveDriveConstants.kDriveKinematics,

// // Position controllers
// new PIDController(AutoConstants.kPXController, 1, AutoConstants.kDXController),
// new PIDController(AutoConstants.kPYController, 1, AutoConstants.kDYController), theta,

// swerveDrive::setModuleStates,

// swerveDrive

// );

// addSequential(swerveControllerCommand1);


// }

// }
Loading