Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

camera + better auto

Browse Source
This commit is contained in:
aarav18
2022-03-25 08:34:25 -06:00
parent 8c6afd8c8a
commit a9ad06d620
3 changed files with 55 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+2
View File
@@ -132,6 +132,8 @@ public final class Constants {
// misc
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
public static final Pose2d HUB_POSE = new Pose2d(new Translation2d(0, 0), new Rotation2d(0));
public static final double AUTO_INCHES_PER_SECOND_AT_FULL_POWER = 134.0;
}
public static final class SerializerConstants {
src/main/java/frc4388/robot/RobotContainer.java
+12 -12
View File
@@ -262,12 +262,14 @@ public class RobotContainer {
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); }
}, m_robotHood));
//Climber Manual
// //Climber Manual
m_robotClimber.setDefaultCommand(
new RunCommand(() -> {
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0.0); }
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setMotors(-getOperatorController().getRightY()); }
}, m_robotClimber));
// m_robotClimber.setDefaultCommand(
// new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getRightY()), m_robotClimber));
m_robotBoomBoom.setDefaultCommand(
new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom)
@@ -498,17 +500,16 @@ public class RobotContainer {
// * 3a. try permanently setting drive motors to brake, not coast, in RobotMap.java, and ask the driver how it feels.
// * 3b. try to only set the drive motors to brake if in auto mode.
// * 4. try new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive);
// ! 1.0 input, 1 second: 134 inches
// ! 0.75 input, 1 second: 48 inches
// ? positive leftY went left, negative leftY went right?
// TODO: if line 372 is true, switch joystick inputs accordingly
// ? 1.0 input, 1 second: 134 inches
// ? 0.75 input, 1 second: 48 inches
// ! POSITIVE Y IS LEFT, POSITIVE X IS BACKWARDS
double turretDistanceFromFront = 10.0; // * distance of turret from the front of the robot in inches. might need to be somewhat accurate.
double distancePerSecond = 134.0; // * assuming emulated joystick input magnitude is 1.0
double offset = 10.0; // * distance (in inches) from ball that we actually want to stop
// ! ball positions are "unit tested"
Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00); // * position of first ball, relative to hub.
Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub.
@@ -519,13 +520,12 @@ public class RobotContainer {
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.0)
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
// ! POSITIVE Y IS LEFT, POSITIVE X IS BACKWARDS
// ! DRIVE BACKWARDS AND SHOOT
return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {1.0, 0.0, 0.0, 0.0}, 0.269), // * go backwards three feet
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {1.0, 0.0, 0.0, 0.0}, (3.0 * 12) / SwerveDriveConstants.AUTO_INCHES_PER_SECOND_AT_FULL_POWER),//0.269), // * go backwards three feet
new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), // * brake
weirdAutoShootingGroup, // * shoot
new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage) // * stop running storage
);
@@ -552,7 +552,7 @@ public class RobotContainer {
// new ParallelCommandGroup( new RunCommand(() -> m_robotIntake.runAtOutput(1.0), m_robotIntake), // * run intake all throughout path
// new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 1.0, 0.0, 0.0}, (40.44 - offset) / distancePerSecond), // * drive to first ball
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {-1.0, 0.0, 0.0, 0.0}, (40.44 - offset) / distancePerSecond), // * drive to first ball
// // new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, true)), // * brake (see line 376),
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
src/main/java/frc4388/robot/subsystems/Camera.java
+41
View File
@@ -0,0 +1,41 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.subsystems;
import edu.wpi.first.cscore.*;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Camera extends SubsystemBase {
/**
* Creates a new Camera.
* Makes a Camera and sends the stream to a CameraServer, to be viewed in Shuffle Board.
* @param name Name of the Camera in Shuffle Board.
* @param id USB Id of the Camera.
* @param width Resolution width.
* @param height Resolution height.
* @param brightness Percent brightness of the stream.
*/
public Camera(String name, int id, int width, int height, int brightness) {
try{
UsbCamera cam = new UsbCamera(name, id);
cam.setResolution(width, height);
cam.setBrightness(brightness);
cam.setFPS(10);
VideoSource camera = cam;
CameraServer.startAutomaticCapture(camera);
}
catch(Exception e) {
System.err.println("Camera broken, pls nerf");
e.printStackTrace();
}
}
@Override
public void periodic() {}
}