From 3d4ee5e351f079f3ad40f2b736ce272e30cce212 Mon Sep 17 00:00:00 2001 From: davidliu-2706 Date: Sat, 7 Mar 2020 11:46:49 -0500 Subject: [PATCH 1/7] Vision integration, a new run command to just report calculated RPM --- src/main/java/frc/robot/RobotContainer.java | 14 ++- .../robot/commands/VisionAssistedShooter.java | 76 +++++++----- .../commands/VisionAssistedTargetRPM.java | 110 ++++++++++++++++++ src/main/java/frc/robot/config/Config.java | 2 +- 4 files changed, 167 insertions(+), 35 deletions(-) create mode 100644 src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index adf814b..6f286ac 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,10 +18,8 @@ import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.DriveBase; import frc.robot.subsystems.DriveBaseHolder; -import frc.robot.commands.ArcadeDriveWithJoystick; -import frc.robot.commands.DriveWithTime; -import frc.robot.commands.SensitiveDriverControl; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import java.util.logging.Logger; @@ -47,10 +45,11 @@ public class RobotContainer { private Command intakeCommand; private Command emptyFeederCommand; private Command reverseFeeder; - private Command sensitiveDriverControlCommand; + private Command sensitiveDriverControlCommand; private Command moveArm; private Command rampShooterCommand; private Command incrementFeeder; + private VisionAssistedTargetRPM targetRPM; private Logger logger = Logger.getLogger("RobotContainer"); private final double AUTO_DRIVE_TIME = 1.0; private final double AUTO_LEFT_MOTOR_SPEED = 0.2; @@ -105,6 +104,13 @@ private void configureButtonBindings() { JoystickButton turnToYaw = new JoystickButton(driverStick, XboxController.Button.kA.value); turnToYaw.whenPressed(new TurnToOuterPortCommand(true, Config.maxYawErrorOuterPortCommand.get(), Config.maxTimeOuterPortCommand.get())); + + //todo: + // targetRPMShooter = new JoystickButton(driverStick, XboxController.Button.kA.value); + //targetRPMShooter.whenPressed(new VisionAssistedShooter( )); + + targetRPM = new VisionAssistedTargetRPM(); + new RunCommand(targetRPM); } /** diff --git a/src/main/java/frc/robot/commands/VisionAssistedShooter.java b/src/main/java/frc/robot/commands/VisionAssistedShooter.java index a3a8d6a..9e3d22c 100644 --- a/src/main/java/frc/robot/commands/VisionAssistedShooter.java +++ b/src/main/java/frc/robot/commands/VisionAssistedShooter.java @@ -1,3 +1,4 @@ +package frc.robot.commands; import frc.robot.config.Config; @@ -5,6 +6,7 @@ import frc.robot.subsystems.ShooterSubsystem; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.nettables.VisionCtrlNetTable; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * VisionAssistedShooter command. @@ -14,17 +16,21 @@ public class VisionAssistedShooter extends CommandBase { //subsystem private final ShooterSubsystem shooter; - + //todo: can be configured in config file as well - //todo: measure the radius for the shooting wheel - private final double SHOOTER_ANGLE_IN_DEGREES = 60.0; - private final double TARGET_HEIGHT_IN_METERS = 2.49; - private final double SHOOTER_WHEEL_RADIUS_IN_CM = 10; + private final double SHOOTER_ANGLE_IN_DEGREES = 46.88; + private final double TARGET_HEIGHT_IN_METERS = 2.02; + private final double SHOOTER_WHEEL_RADIUS_IN_CM = 5.5; + private final double HALF_OF_GRAVITY = 4.91; private final double CONVERSION_NUMBER = 3000; + private final double METER_PER_FOOT = 0.3048; // values from Vision Network Table - private double distanceToOuterPort; + private double distanceToOuterPortInMeters; + + //adjusted value + private double adjustedDistanceToOutPortInMeters; //network table for vision control private VisionCtrlNetTable visionControlNetTable; @@ -37,15 +43,16 @@ public class VisionAssistedShooter extends CommandBase { /** * Creates a new VisionAssistedShooter Command. * - * @param subsystem The subsystem used by this command. */ - public VisionAssistedShooter(ShooterSubsystem subsystem) { + public VisionAssistedShooter( ) { //subsystem - shooter = subsystem; + shooter = ShooterSubsystem.getInstance(); - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(subsystem); + if (shooter.isActive()){ + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(shooter); + } } @Override @@ -54,18 +61,25 @@ public void initialize() { // Ensure the vision is running in tape mode visionControlNetTable.setTapeMode(); - //Read the network table from vision to get the distance from the power port. - //distance from vision - //NOTE: unit should be meter. If not, need conversion here. - distanceToOuterPort = visionControlNetTable.distanceToOuterPort.get(); + //Read the network table from vision to get the distance from the target. + distanceToOuterPortInMeters = visionControlNetTable.distanceToOuterPort.get(); + if ( distanceToOuterPortInMeters < 0.0 ) + { + //Vision can not provide valid detection + targetRPM = 0; + } + else + { + //NOTE: unit in the vision network table is feet. Convert it to meters. + distanceToOuterPortInMeters = distanceToOuterPortInMeters * METER_PER_FOOT ; - //todo: to adjuste the distance for the shooter - //targetDistance + //todo: to adjuste the distance for the shooter + //adjustedDistanceToOutPortInMeters = ; - //Calculate the RPM of the shooter wheel. - double targetV = initVelocity( distanceToOuterPort); - targetRPM = velocityToRPM (targetV); - + //Calculate the RPM of the shooter wheel. + double targetV = initVelocity( distanceToOuterPortInMeters); + targetRPM = velocityToRPM (targetV); + } } @Override @@ -75,7 +89,9 @@ public void execute() { shooter.setTargetRPM((int) targetRPM); //todo: provide feedback to the shuffleboard for Driver Team - + //vision assisted RPM + + SmartDashboard.putNumber("Vision Assisted Target Shooter RPM", targetRPM); } @Override @@ -91,22 +107,22 @@ public void end(boolean interrupted) { double initVelocity(double distanceToTargetInMeters) { - double dCheck = Math.tan(SHOOTER_ANGLE_IN_DEGREES)*distanceToTargetInMeters - TARGET_HEIGHT_IN_METERS; double dTemp; - //unit: m/s double dInitVelocity; + + double dCheck = Math.tan(SHOOTER_ANGLE_IN_DEGREES)*distanceToTargetInMeters - TARGET_HEIGHT_IN_METERS; if (dCheck > 0) { - dTemp = Math.sqrt(HALF_OF_GRAVITY/dCheck); - dInitVelocity = distanceToTargetInMeters/Math.cos(SHOOTER_ANGLE_IN_DEGREES) * dTemp; + dTemp = Math.sqrt(HALF_OF_GRAVITY/dCheck); + dInitVelocity = distanceToTargetInMeters/Math.cos(SHOOTER_ANGLE_IN_DEGREES) * dTemp; } else { - dInitVelocity = 0.0; - System.out.println("WARNING! Not suitable for shooting!"); - } - + dInitVelocity = 0.0; + System.out.println("WARNING! Not suitable for shooting!"); + } + return dInitVelocity; } diff --git a/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java b/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java new file mode 100644 index 0000000..615325b --- /dev/null +++ b/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java @@ -0,0 +1,110 @@ +package frc.robot.commands; + +import frc.robot.config.Config; + +import frc.robot.subsystems.DriveBase; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.nettables.VisionCtrlNetTable; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * VisionAssistedTargetRPM command. + * NOTE: this command should be run after command TurnToOuterPortCommand + */ +public class VisionAssistedTargetRPM implements Runnable { + + //subsystem + + //todo: can be configured in config file as well + private final double SHOOTER_ANGLE_IN_DEGREES = 46.88; + private final double TARGET_HEIGHT_IN_METERS = 2.02; + private final double SHOOTER_WHEEL_RADIUS_IN_CM = 5.5; + + private final double HALF_OF_GRAVITY = 4.91; + private final double CONVERSION_NUMBER = 3000; + private final double METER_PER_FOOT = 0.3048; + + // values from Vision Network Table + private double distanceToOuterPortInMeters; + + //adjusted value + private double adjustedDistanceToOutPortInMeters; + + //network table for vision control + private VisionCtrlNetTable visionControlNetTable; + + //calculated for the shooter + double targetDistance = 0; + double targetRPM = 0; + + + /** + * Creates a new VisionAssistedTargetRPM Command. + * + * @param subsystem The subsystem used by this command. + */ + public VisionAssistedTargetRPM() { + + // Ensure the vision is running in tape mode + visionControlNetTable.setTapeMode(); + + } + + public void run() { + + //Read the network table from vision to get the distance from the target. + distanceToOuterPortInMeters = visionControlNetTable.distanceToOuterPort.get(); + if ( distanceToOuterPortInMeters < 0.0 ) + { + //Vision can not provide valid detection + targetRPM = 0; + } + else + { + //NOTE: unit in the vision network table is feet. Convert it to meters. + distanceToOuterPortInMeters = distanceToOuterPortInMeters * METER_PER_FOOT ; + + //todo: to adjuste the distance for the shooter + //adjustedDistanceToOutPortInMeters = ; + + //Calculate the RPM of the shooter wheel. + double targetV = initVelocity( distanceToOuterPortInMeters); + targetRPM = velocityToRPM (targetV); + } + + // provide feedback to the shuffleboard for Driver Team + // vision assisted RPM + SmartDashboard.putNumber("Info: Vision Assisted Target RPM", targetRPM); + } + + private double initVelocity(double distanceToTargetInMeters) { + double dTemp; + //unit: m/s + double dInitVelocity; + + double dCheck = Math.tan(SHOOTER_ANGLE_IN_DEGREES)*distanceToTargetInMeters - TARGET_HEIGHT_IN_METERS; + if (dCheck > 0) + { + dTemp = Math.sqrt(HALF_OF_GRAVITY/dCheck); + dInitVelocity = distanceToTargetInMeters/Math.cos(SHOOTER_ANGLE_IN_DEGREES) * dTemp; + } + else + { + dInitVelocity = 0.0; + System.out.println("WARNING! Not suitable for shooting!"); + } + + return dInitVelocity; + + } + + // convert velocity to RPM + // velocity: unit m/s + // return: unit revolutions per minute +private double velocityToRPM( double velocity) + { + double rpm = velocity*CONVERSION_NUMBER/(Math.PI*SHOOTER_WHEEL_RADIUS_IN_CM); + return rpm; + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/config/Config.java b/src/main/java/frc/robot/config/Config.java index c0e501f..c5f6eba 100644 --- a/src/main/java/frc/robot/config/Config.java +++ b/src/main/java/frc/robot/config/Config.java @@ -134,7 +134,7 @@ private Config() { // Vision Table Constants public static String VISION_TABLE_NAME = "MergeVision"; public static String DISTANCE_POWERCELL = "DistanceToPowerCell"; - public static String DISTANCE_OUTER_PORT = "DistanceToOuterPort"; + public static String DISTANCE_OUTER_PORT = "DistanceToTarget"; public static String YAW_POWERCELL = "YawToPowerCell"; public static String YAW_OUTER_PORT = "YawToTarget"; From e3e1dfa2c4af8ec85f40fef363cef078dc504b10 Mon Sep 17 00:00:00 2001 From: davidliu-2706 Date: Sat, 7 Mar 2020 15:07:54 -0500 Subject: [PATCH 2/7] commands/VisionAssistedTargetRPM.java --- .../commands/VisionAssistedTargetRPM.java | 45 +++++++++++++------ 1 file changed, 31 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java b/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java index 615325b..f40797c 100644 --- a/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java +++ b/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java @@ -19,6 +19,8 @@ public class VisionAssistedTargetRPM implements Runnable { private final double SHOOTER_ANGLE_IN_DEGREES = 46.88; private final double TARGET_HEIGHT_IN_METERS = 2.02; private final double SHOOTER_WHEEL_RADIUS_IN_CM = 5.5; + private final double D_CAMERA_SHOOTER_IN_METERS = 0.26; + private final double ANGLE_CAMERA_SHOOTER = 46.37; private final double HALF_OF_GRAVITY = 4.91; private final double CONVERSION_NUMBER = 3000; @@ -26,6 +28,7 @@ public class VisionAssistedTargetRPM implements Runnable { // values from Vision Network Table private double distanceToOuterPortInMeters; + private double yawToOutperPort; //adjusted value private double adjustedDistanceToOutPortInMeters; @@ -52,25 +55,39 @@ public VisionAssistedTargetRPM() { public void run() { - //Read the network table from vision to get the distance from the target. - distanceToOuterPortInMeters = visionControlNetTable.distanceToOuterPort.get(); - if ( distanceToOuterPortInMeters < 0.0 ) + yawToOutperPort = visionControlNetTable.yawToOuterPort.get(); + + //Read the network table from vision to get the distance from the target. + distanceToOuterPortInMeters = visionControlNetTable.distanceToOuterPort.get(); + if ( distanceToOuterPortInMeters < 0.0 ) + { + //Vision can not provide valid detection + targetRPM = 0; + } + else + { + //NOTE: unit in the vision network table is feet. Convert it to meters. + distanceToOuterPortInMeters = distanceToOuterPortInMeters * METER_PER_FOOT ; + + //todo: to adjuste the distance for the shooter + //adjustedDistanceToOutPortInMeters = ; + double temp = 2*distanceToOuterPortInMeters * Math.cos(180-ANGLE_CAMERA_SHOOTER); + double check = distanceToOuterPortInMeters*distanceToOuterPortInMeters + + D_CAMERA_SHOOTER_IN_METERS*D_CAMERA_SHOOTER_IN_METERS - temp; + if (check<0) { - //Vision can not provide valid detection - targetRPM = 0; + adjustedDistanceToOutPortInMeters = distanceToOuterPortInMeters; } else { - //NOTE: unit in the vision network table is feet. Convert it to meters. - distanceToOuterPortInMeters = distanceToOuterPortInMeters * METER_PER_FOOT ; - - //todo: to adjuste the distance for the shooter - //adjustedDistanceToOutPortInMeters = ; - - //Calculate the RPM of the shooter wheel. - double targetV = initVelocity( distanceToOuterPortInMeters); - targetRPM = velocityToRPM (targetV); + adjustedDistanceToOutPortInMeters = Math.sqrt(check); + } + + //Calculate the RPM of the shooter wheel. + double targetV = initVelocity( adjustedDistanceToOutPortInMeters); + targetRPM = velocityToRPM (targetV); + } // provide feedback to the shuffleboard for Driver Team // vision assisted RPM From 1c0c91a500ad5b4e9bdb20cafa3024ee70155025 Mon Sep 17 00:00:00 2001 From: davidliu-2706 Date: Mon, 9 Mar 2020 18:06:53 -0400 Subject: [PATCH 3/7] adjusted vision distance --- .../robot/commands/VisionAssistedShooter.java | 12 ++--- .../commands/VisionAssistedTargetRPM.java | 44 ++++++------------- 2 files changed, 21 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/commands/VisionAssistedShooter.java b/src/main/java/frc/robot/commands/VisionAssistedShooter.java index 9e3d22c..926a292 100644 --- a/src/main/java/frc/robot/commands/VisionAssistedShooter.java +++ b/src/main/java/frc/robot/commands/VisionAssistedShooter.java @@ -21,6 +21,7 @@ public class VisionAssistedShooter extends CommandBase { private final double SHOOTER_ANGLE_IN_DEGREES = 46.88; private final double TARGET_HEIGHT_IN_METERS = 2.02; private final double SHOOTER_WHEEL_RADIUS_IN_CM = 5.5; + private final double D_CAMERA_SHOOTER_IN_METERS = 0.26; private final double HALF_OF_GRAVITY = 4.91; private final double CONVERSION_NUMBER = 3000; @@ -63,6 +64,7 @@ public void initialize() { //Read the network table from vision to get the distance from the target. distanceToOuterPortInMeters = visionControlNetTable.distanceToOuterPort.get(); + if ( distanceToOuterPortInMeters < 0.0 ) { //Vision can not provide valid detection @@ -73,12 +75,12 @@ public void initialize() { //NOTE: unit in the vision network table is feet. Convert it to meters. distanceToOuterPortInMeters = distanceToOuterPortInMeters * METER_PER_FOOT ; - //todo: to adjuste the distance for the shooter - //adjustedDistanceToOutPortInMeters = ; + // adjuste the distance for the shooter + adjustedDistanceToOutPortInMeters = distanceToOuterPortInMeters + D_CAMERA_SHOOTER_IN_METERS; //Calculate the RPM of the shooter wheel. - double targetV = initVelocity( distanceToOuterPortInMeters); - targetRPM = velocityToRPM (targetV); + double targetV = initVelocity( adjustedDistanceToOutPortInMeters); + targetRPM = velocityToRPM (targetV); } } @@ -91,7 +93,7 @@ public void execute() { //todo: provide feedback to the shuffleboard for Driver Team //vision assisted RPM - SmartDashboard.putNumber("Vision Assisted Target Shooter RPM", targetRPM); + SmartDashboard.putNumber("Vision Assisted Shooter RPM", targetRPM); } @Override diff --git a/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java b/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java index f40797c..3bae53d 100644 --- a/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java +++ b/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java @@ -2,7 +2,7 @@ import frc.robot.config.Config; -import frc.robot.subsystems.DriveBase; +//import frc.robot.subsystems.DriveBase; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.nettables.VisionCtrlNetTable; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -20,27 +20,24 @@ public class VisionAssistedTargetRPM implements Runnable { private final double TARGET_HEIGHT_IN_METERS = 2.02; private final double SHOOTER_WHEEL_RADIUS_IN_CM = 5.5; private final double D_CAMERA_SHOOTER_IN_METERS = 0.26; - private final double ANGLE_CAMERA_SHOOTER = 46.37; private final double HALF_OF_GRAVITY = 4.91; private final double CONVERSION_NUMBER = 3000; private final double METER_PER_FOOT = 0.3048; - + // values from Vision Network Table private double distanceToOuterPortInMeters; - private double yawToOutperPort; - + //adjusted value private double adjustedDistanceToOutPortInMeters; //network table for vision control private VisionCtrlNetTable visionControlNetTable; - //calculated for the shooter + //calculated RPM double targetDistance = 0; double targetRPM = 0; - /** * Creates a new VisionAssistedTargetRPM Command. * @@ -55,43 +52,30 @@ public VisionAssistedTargetRPM() { public void run() { - yawToOutperPort = visionControlNetTable.yawToOuterPort.get(); - - //Read the network table from vision to get the distance from the target. + //Read the network table from vision distanceToOuterPortInMeters = visionControlNetTable.distanceToOuterPort.get(); + if ( distanceToOuterPortInMeters < 0.0 ) { - //Vision can not provide valid detection + // Vision can not provide valid detection targetRPM = 0; } else { - //NOTE: unit in the vision network table is feet. Convert it to meters. + // NOTE: unit in the vision network table is feet. Convert it to meters. distanceToOuterPortInMeters = distanceToOuterPortInMeters * METER_PER_FOOT ; - //todo: to adjuste the distance for the shooter - //adjustedDistanceToOutPortInMeters = ; - double temp = 2*distanceToOuterPortInMeters * Math.cos(180-ANGLE_CAMERA_SHOOTER); - double check = distanceToOuterPortInMeters*distanceToOuterPortInMeters - + D_CAMERA_SHOOTER_IN_METERS*D_CAMERA_SHOOTER_IN_METERS - temp; - if (check<0) - { - adjustedDistanceToOutPortInMeters = distanceToOuterPortInMeters; - } - else - { - adjustedDistanceToOutPortInMeters = Math.sqrt(check); - - } - + // adjuste the distance for the shooter + adjustedDistanceToOutPortInMeters = distanceToOuterPortInMeters + D_CAMERA_SHOOTER_IN_METERS; + //Calculate the RPM of the shooter wheel. double targetV = initVelocity( adjustedDistanceToOutPortInMeters); - targetRPM = velocityToRPM (targetV); + targetRPM = velocityToRPM (targetV); } // provide feedback to the shuffleboard for Driver Team - // vision assisted RPM - SmartDashboard.putNumber("Info: Vision Assisted Target RPM", targetRPM); + // vision assisted calculated RPM + SmartDashboard.putNumber("Info: Vision Assisted Calculated Target RPM", targetRPM); } private double initVelocity(double distanceToTargetInMeters) { From d9b8598f276b38f45f649b351843606d022b1a80 Mon Sep 17 00:00:00 2001 From: davidliu-2706 Date: Mon, 9 Mar 2020 19:55:12 -0400 Subject: [PATCH 4/7] Cleaned up --- src/main/java/frc/robot/commands/VisionAssistedShooter.java | 3 --- src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java | 4 ---- 2 files changed, 7 deletions(-) diff --git a/src/main/java/frc/robot/commands/VisionAssistedShooter.java b/src/main/java/frc/robot/commands/VisionAssistedShooter.java index 926a292..9684500 100644 --- a/src/main/java/frc/robot/commands/VisionAssistedShooter.java +++ b/src/main/java/frc/robot/commands/VisionAssistedShooter.java @@ -1,8 +1,6 @@ package frc.robot.commands; import frc.robot.config.Config; - -import frc.robot.subsystems.DriveBase; import frc.robot.subsystems.ShooterSubsystem; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.nettables.VisionCtrlNetTable; @@ -107,7 +105,6 @@ public void end(boolean interrupted) { } - double initVelocity(double distanceToTargetInMeters) { double dTemp; //unit: m/s diff --git a/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java b/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java index 3bae53d..adff35f 100644 --- a/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java +++ b/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java @@ -1,8 +1,6 @@ package frc.robot.commands; import frc.robot.config.Config; - -//import frc.robot.subsystems.DriveBase; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.nettables.VisionCtrlNetTable; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -12,8 +10,6 @@ * NOTE: this command should be run after command TurnToOuterPortCommand */ public class VisionAssistedTargetRPM implements Runnable { - - //subsystem //todo: can be configured in config file as well private final double SHOOTER_ANGLE_IN_DEGREES = 46.88; From 14ff77ee5e90f694ebdd8bc3c0312a82a3e82431 Mon Sep 17 00:00:00 2001 From: davidliu-2706 Date: Tue, 10 Mar 2020 20:22:05 -0400 Subject: [PATCH 5/7] Adressed the review comments --- .../robot/commands/VisionAssistedShooter.java | 28 ++++----------- .../commands/VisionAssistedTargetRPM.java | 35 ++++++++----------- src/main/java/frc/robot/config/Config.java | 11 +++++- 3 files changed, 31 insertions(+), 43 deletions(-) diff --git a/src/main/java/frc/robot/commands/VisionAssistedShooter.java b/src/main/java/frc/robot/commands/VisionAssistedShooter.java index 9684500..67e5153 100644 --- a/src/main/java/frc/robot/commands/VisionAssistedShooter.java +++ b/src/main/java/frc/robot/commands/VisionAssistedShooter.java @@ -14,16 +14,6 @@ public class VisionAssistedShooter extends CommandBase { //subsystem private final ShooterSubsystem shooter; - - //todo: can be configured in config file as well - private final double SHOOTER_ANGLE_IN_DEGREES = 46.88; - private final double TARGET_HEIGHT_IN_METERS = 2.02; - private final double SHOOTER_WHEEL_RADIUS_IN_CM = 5.5; - private final double D_CAMERA_SHOOTER_IN_METERS = 0.26; - - private final double HALF_OF_GRAVITY = 4.91; - private final double CONVERSION_NUMBER = 3000; - private final double METER_PER_FOOT = 0.3048; // values from Vision Network Table private double distanceToOuterPortInMeters; @@ -32,7 +22,7 @@ public class VisionAssistedShooter extends CommandBase { private double adjustedDistanceToOutPortInMeters; //network table for vision control - private VisionCtrlNetTable visionControlNetTable; + private VisionCtrlNetTable visionControlNetTable = new VisionCtrlNetTable (); //calculated for the shooter double targetDistance = 0; @@ -57,9 +47,6 @@ public VisionAssistedShooter( ) { @Override public void initialize() { - // Ensure the vision is running in tape mode - visionControlNetTable.setTapeMode(); - //Read the network table from vision to get the distance from the target. distanceToOuterPortInMeters = visionControlNetTable.distanceToOuterPort.get(); @@ -71,10 +58,10 @@ public void initialize() { else { //NOTE: unit in the vision network table is feet. Convert it to meters. - distanceToOuterPortInMeters = distanceToOuterPortInMeters * METER_PER_FOOT ; + distanceToOuterPortInMeters = distanceToOuterPortInMeters * Config.METER_PER_FOOT ; // adjuste the distance for the shooter - adjustedDistanceToOutPortInMeters = distanceToOuterPortInMeters + D_CAMERA_SHOOTER_IN_METERS; + adjustedDistanceToOutPortInMeters = distanceToOuterPortInMeters + Config.D_CAMERA_SHOOTER_IN_METERS; //Calculate the RPM of the shooter wheel. double targetV = initVelocity( adjustedDistanceToOutPortInMeters); @@ -110,16 +97,15 @@ public void end(boolean interrupted) { //unit: m/s double dInitVelocity; - double dCheck = Math.tan(SHOOTER_ANGLE_IN_DEGREES)*distanceToTargetInMeters - TARGET_HEIGHT_IN_METERS; + double dCheck = Math.tan(Config.SHOOTER_ANGLE_IN_DEGREES)*distanceToTargetInMeters - Config.TARGET_HEIGHT_IN_METERS; if (dCheck > 0) { - dTemp = Math.sqrt(HALF_OF_GRAVITY/dCheck); - dInitVelocity = distanceToTargetInMeters/Math.cos(SHOOTER_ANGLE_IN_DEGREES) * dTemp; + dTemp = Math.sqrt(Config.HALF_OF_GRAVITY/dCheck); + dInitVelocity = distanceToTargetInMeters/Math.cos(Config.SHOOTER_ANGLE_IN_DEGREES) * dTemp; } else { dInitVelocity = 0.0; - System.out.println("WARNING! Not suitable for shooting!"); } return dInitVelocity; @@ -131,7 +117,7 @@ public void end(boolean interrupted) { // return: unit revolutions per minute double velocityToRPM( double velocity) { - double rpm = velocity*CONVERSION_NUMBER/(Math.PI*SHOOTER_WHEEL_RADIUS_IN_CM); + double rpm = velocity*Config.CONVERSION_NUMBER/(Math.PI*Config.SHOOTER_WHEEL_RADIUS_IN_CM); return rpm; } diff --git a/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java b/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java index adff35f..cf4ab2f 100644 --- a/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java +++ b/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java @@ -1,7 +1,7 @@ package frc.robot.commands; import frc.robot.config.Config; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.RunCommand; import frc.robot.nettables.VisionCtrlNetTable; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -11,16 +11,7 @@ */ public class VisionAssistedTargetRPM implements Runnable { - //todo: can be configured in config file as well - private final double SHOOTER_ANGLE_IN_DEGREES = 46.88; - private final double TARGET_HEIGHT_IN_METERS = 2.02; - private final double SHOOTER_WHEEL_RADIUS_IN_CM = 5.5; - private final double D_CAMERA_SHOOTER_IN_METERS = 0.26; - - private final double HALF_OF_GRAVITY = 4.91; - private final double CONVERSION_NUMBER = 3000; - private final double METER_PER_FOOT = 0.3048; - + // values from Vision Network Table private double distanceToOuterPortInMeters; @@ -28,7 +19,7 @@ public class VisionAssistedTargetRPM implements Runnable { private double adjustedDistanceToOutPortInMeters; //network table for vision control - private VisionCtrlNetTable visionControlNetTable; + private VisionCtrlNetTable visionControlNetTable = new VisionCtrlNetTable (); //calculated RPM double targetDistance = 0; @@ -42,8 +33,8 @@ public class VisionAssistedTargetRPM implements Runnable { public VisionAssistedTargetRPM() { // Ensure the vision is running in tape mode - visionControlNetTable.setTapeMode(); - + //don't need this any more + //VisionCtrlNetTable.setTapeMode(); } public void run() { @@ -59,19 +50,21 @@ public void run() { else { // NOTE: unit in the vision network table is feet. Convert it to meters. - distanceToOuterPortInMeters = distanceToOuterPortInMeters * METER_PER_FOOT ; + distanceToOuterPortInMeters = distanceToOuterPortInMeters * Config.METER_PER_FOOT ; // adjuste the distance for the shooter - adjustedDistanceToOutPortInMeters = distanceToOuterPortInMeters + D_CAMERA_SHOOTER_IN_METERS; + adjustedDistanceToOutPortInMeters = distanceToOuterPortInMeters + Config.D_CAMERA_SHOOTER_IN_METERS; //Calculate the RPM of the shooter wheel. double targetV = initVelocity( adjustedDistanceToOutPortInMeters); targetRPM = velocityToRPM (targetV); } + System.out.println("Vision RPM"); + System.out.println( targetRPM); // provide feedback to the shuffleboard for Driver Team // vision assisted calculated RPM - SmartDashboard.putNumber("Info: Vision Assisted Calculated Target RPM", targetRPM); + SmartDashboard.putNumber("Vision RPM", targetRPM); } private double initVelocity(double distanceToTargetInMeters) { @@ -79,11 +72,11 @@ private double initVelocity(double distanceToTargetInMeters) { //unit: m/s double dInitVelocity; - double dCheck = Math.tan(SHOOTER_ANGLE_IN_DEGREES)*distanceToTargetInMeters - TARGET_HEIGHT_IN_METERS; + double dCheck = Math.tan(Config.SHOOTER_ANGLE_IN_DEGREES)*distanceToTargetInMeters - Config.TARGET_HEIGHT_IN_METERS; if (dCheck > 0) { - dTemp = Math.sqrt(HALF_OF_GRAVITY/dCheck); - dInitVelocity = distanceToTargetInMeters/Math.cos(SHOOTER_ANGLE_IN_DEGREES) * dTemp; + dTemp = Math.sqrt(Config.HALF_OF_GRAVITY/dCheck); + dInitVelocity = distanceToTargetInMeters/Math.cos(Config.SHOOTER_ANGLE_IN_DEGREES) * dTemp; } else { @@ -100,7 +93,7 @@ private double initVelocity(double distanceToTargetInMeters) { // return: unit revolutions per minute private double velocityToRPM( double velocity) { - double rpm = velocity*CONVERSION_NUMBER/(Math.PI*SHOOTER_WHEEL_RADIUS_IN_CM); + double rpm = velocity*Config.CONVERSION_NUMBER/(Math.PI*Config.SHOOTER_WHEEL_RADIUS_IN_CM); return rpm; } diff --git a/src/main/java/frc/robot/config/Config.java b/src/main/java/frc/robot/config/Config.java index 0097860..a0dc1ee 100644 --- a/src/main/java/frc/robot/config/Config.java +++ b/src/main/java/frc/robot/config/Config.java @@ -140,12 +140,21 @@ private Config() { public static NetworkTable constantsTable = NetworkTableInstance.getDefault().getTable("constants"); // Vision Table Constants - public static String VISION_TABLE_NAME = "MergeVision"; + public static String VISION_TABLE_NAME = "MergeVisionPipelinePi20"; public static String DISTANCE_POWERCELL = "DistanceToPowerCell"; public static String DISTANCE_OUTER_PORT = "DistanceToTarget"; public static String YAW_POWERCELL = "YawToPowerCell"; public static String YAW_OUTER_PORT = "YawToTarget"; + // Vision Assisted RPM + public static double SHOOTER_ANGLE_IN_DEGREES = 46.88; + public static double TARGET_HEIGHT_IN_METERS = 2.02; + public static double SHOOTER_WHEEL_RADIUS_IN_CM = 5.5; + public static double D_CAMERA_SHOOTER_IN_METERS = 0.26; + public static double HALF_OF_GRAVITY = 4.91; + public static double CONVERSION_NUMBER = 3000; + public static double METER_PER_FOOT = 0.3048; + // Drivetrain PID values public static double DRIVETRAIN_P_SPECIFIC = robotSpecific(0.022, 0.0, 0.0, 0.018d, 0.0, 0.25); public static double DRIVETRAIN_D_SPECIFIC = robotSpecific(0.0028, 0.0, 0.0, 0.0016d, 0.0, 0.03); From 139caaa7088b4113d6c6197cbb98a4ce014b6f05 Mon Sep 17 00:00:00 2001 From: davidliu-2706 Date: Tue, 10 Mar 2020 21:59:24 -0400 Subject: [PATCH 6/7] clean up --- src/main/java/frc/robot/RobotContainer.java | 1 + .../robot/commands/VisionAssistedShooter.java | 37 ++----------------- .../commands/VisionAssistedTargetRPM.java | 9 ++--- 3 files changed, 8 insertions(+), 39 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5aeeada..2b17759 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -53,6 +53,7 @@ public class RobotContainer { private Command incrementFeeder; private Command moveArm; private Command sensitiveDriving; + private Runnable targetRPM; private Logger logger = Logger.getLogger("RobotContainer"); private final double AUTO_DRIVE_TIME = 1.0; private final double AUTO_LEFT_MOTOR_SPEED = 0.2; diff --git a/src/main/java/frc/robot/commands/VisionAssistedShooter.java b/src/main/java/frc/robot/commands/VisionAssistedShooter.java index 67e5153..b4e6d46 100644 --- a/src/main/java/frc/robot/commands/VisionAssistedShooter.java +++ b/src/main/java/frc/robot/commands/VisionAssistedShooter.java @@ -64,8 +64,8 @@ public void initialize() { adjustedDistanceToOutPortInMeters = distanceToOuterPortInMeters + Config.D_CAMERA_SHOOTER_IN_METERS; //Calculate the RPM of the shooter wheel. - double targetV = initVelocity( adjustedDistanceToOutPortInMeters); - targetRPM = velocityToRPM (targetV); + double targetV = VisionAssistedTargetRPM.initVelocity( adjustedDistanceToOutPortInMeters); + targetRPM = VisionAssistedTargetRPM .velocityToRPM (targetV); } } @@ -75,9 +75,8 @@ public void execute() { //Set the shooter to the target RPM. shooter.setTargetRPM((int) targetRPM); - //todo: provide feedback to the shuffleboard for Driver Team - //vision assisted RPM - + // provide feedback to the shuffleboard for Driver Team + // vision assisted RPM SmartDashboard.putNumber("Vision Assisted Shooter RPM", targetRPM); } @@ -92,33 +91,5 @@ public void end(boolean interrupted) { } - double initVelocity(double distanceToTargetInMeters) { - double dTemp; - //unit: m/s - double dInitVelocity; - - double dCheck = Math.tan(Config.SHOOTER_ANGLE_IN_DEGREES)*distanceToTargetInMeters - Config.TARGET_HEIGHT_IN_METERS; - if (dCheck > 0) - { - dTemp = Math.sqrt(Config.HALF_OF_GRAVITY/dCheck); - dInitVelocity = distanceToTargetInMeters/Math.cos(Config.SHOOTER_ANGLE_IN_DEGREES) * dTemp; - } - else - { - dInitVelocity = 0.0; - } - - return dInitVelocity; - } - - // convert velocity to RPM - // velocity: unit m/s - // return: unit revolutions per minute -double velocityToRPM( double velocity) - { - double rpm = velocity*Config.CONVERSION_NUMBER/(Math.PI*Config.SHOOTER_WHEEL_RADIUS_IN_CM); - return rpm; - } - } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java b/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java index cf4ab2f..90fb3d1 100644 --- a/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java +++ b/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java @@ -60,14 +60,12 @@ public void run() { targetRPM = velocityToRPM (targetV); } - System.out.println("Vision RPM"); - System.out.println( targetRPM); // provide feedback to the shuffleboard for Driver Team // vision assisted calculated RPM SmartDashboard.putNumber("Vision RPM", targetRPM); } - private double initVelocity(double distanceToTargetInMeters) { + public static double initVelocity(double distanceToTargetInMeters) { double dTemp; //unit: m/s double dInitVelocity; @@ -80,8 +78,7 @@ private double initVelocity(double distanceToTargetInMeters) { } else { - dInitVelocity = 0.0; - System.out.println("WARNING! Not suitable for shooting!"); + dInitVelocity = 0.0; } return dInitVelocity; @@ -91,7 +88,7 @@ private double initVelocity(double distanceToTargetInMeters) { // convert velocity to RPM // velocity: unit m/s // return: unit revolutions per minute -private double velocityToRPM( double velocity) +public static double velocityToRPM( double velocity) { double rpm = velocity*Config.CONVERSION_NUMBER/(Math.PI*Config.SHOOTER_WHEEL_RADIUS_IN_CM); return rpm; From b3e4c4cf9cf02bab3e5c16aeac4ccc8efde4190c Mon Sep 17 00:00:00 2001 From: davidliu-2706 Date: Wed, 11 Mar 2020 07:55:49 -0400 Subject: [PATCH 7/7] Addressed comments --- .../java/frc/robot/commands/VisionAssistedShooter.java | 3 +-- .../java/frc/robot/commands/VisionAssistedTargetRPM.java | 7 ++----- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/commands/VisionAssistedShooter.java b/src/main/java/frc/robot/commands/VisionAssistedShooter.java index b4e6d46..afbd710 100644 --- a/src/main/java/frc/robot/commands/VisionAssistedShooter.java +++ b/src/main/java/frc/robot/commands/VisionAssistedShooter.java @@ -25,7 +25,6 @@ public class VisionAssistedShooter extends CommandBase { private VisionCtrlNetTable visionControlNetTable = new VisionCtrlNetTable (); //calculated for the shooter - double targetDistance = 0; double targetRPM = 0; @@ -65,7 +64,7 @@ public void initialize() { //Calculate the RPM of the shooter wheel. double targetV = VisionAssistedTargetRPM.initVelocity( adjustedDistanceToOutPortInMeters); - targetRPM = VisionAssistedTargetRPM .velocityToRPM (targetV); + targetRPM = VisionAssistedTargetRPM.velocityToRPM (targetV); } } diff --git a/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java b/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java index 90fb3d1..2323ba1 100644 --- a/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java +++ b/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java @@ -22,8 +22,7 @@ public class VisionAssistedTargetRPM implements Runnable { private VisionCtrlNetTable visionControlNetTable = new VisionCtrlNetTable (); //calculated RPM - double targetDistance = 0; - double targetRPM = 0; + double targetRPM = 0; /** * Creates a new VisionAssistedTargetRPM Command. @@ -32,9 +31,7 @@ public class VisionAssistedTargetRPM implements Runnable { */ public VisionAssistedTargetRPM() { - // Ensure the vision is running in tape mode - //don't need this any more - //VisionCtrlNetTable.setTapeMode(); + } public void run() {