diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index af98ba7..2b17759 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,7 @@ import frc.robot.sensors.AnalogSelector; import frc.robot.subsystems.*; import frc.robot.commands.ArcadeDriveWithJoystick; +import edu.wpi.first.wpilibj2.command.RunCommand; import java.util.logging.Logger; @@ -52,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; @@ -117,6 +119,8 @@ private void configureButtonBindings() { sensitiveDriving = new SensitiveDriverControl(driverStick); new JoystickButton(driverStick, XboxController.Button.kBumperLeft.value).whenHeld(sensitiveDriving); + 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 8d31b54..afbd710 100644 --- a/src/main/java/frc/robot/commands/VisionAssistedShooter.java +++ b/src/main/java/frc/robot/commands/VisionAssistedShooter.java @@ -1,11 +1,10 @@ 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; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * VisionAssistedShooter command. @@ -15,58 +14,58 @@ 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 HALF_OF_GRAVITY = 4.91; - private final double CONVERSION_NUMBER = 3000; // values from Vision Network Table - private double distanceToOuterPort; + private double distanceToOuterPortInMeters; + + //adjusted value + private double adjustedDistanceToOutPortInMeters; //network table for vision control - private VisionCtrlNetTable visionControlNetTable; + private VisionCtrlNetTable visionControlNetTable = new VisionCtrlNetTable (); //calculated for the shooter - double targetDistance = 0; double targetRPM = 0; /** * 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 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(); - //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(); + 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 * Config.METER_PER_FOOT ; - //todo: to adjuste the distance for the shooter - //targetDistance + // adjuste the distance for the shooter + adjustedDistanceToOutPortInMeters = distanceToOuterPortInMeters + Config.D_CAMERA_SHOOTER_IN_METERS; - //Calculate the RPM of the shooter wheel. - double targetV = initVelocity( distanceToOuterPort); - targetRPM = velocityToRPM (targetV); - + //Calculate the RPM of the shooter wheel. + double targetV = VisionAssistedTargetRPM.initVelocity( adjustedDistanceToOutPortInMeters); + targetRPM = VisionAssistedTargetRPM.velocityToRPM (targetV); + } } @Override @@ -75,8 +74,9 @@ public void execute() { //Set the shooter to the target RPM. shooter.setTargetRPM((int) targetRPM); - //todo: provide feedback to the shuffleboard for Driver Team - + // provide feedback to the shuffleboard for Driver Team + // vision assisted RPM + SmartDashboard.putNumber("Vision Assisted Shooter RPM", targetRPM); } @Override @@ -90,35 +90,5 @@ 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; - 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 -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/commands/VisionAssistedTargetRPM.java b/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java new file mode 100644 index 0000000..2323ba1 --- /dev/null +++ b/src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java @@ -0,0 +1,94 @@ +package frc.robot.commands; + +import frc.robot.config.Config; +import edu.wpi.first.wpilibj2.command.RunCommand; +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 { + + + // values from Vision Network Table + private double distanceToOuterPortInMeters; + + //adjusted value + private double adjustedDistanceToOutPortInMeters; + + //network table for vision control + private VisionCtrlNetTable visionControlNetTable = new VisionCtrlNetTable (); + + //calculated RPM + double targetRPM = 0; + + /** + * Creates a new VisionAssistedTargetRPM Command. + * + * @param subsystem The subsystem used by this command. + */ + public VisionAssistedTargetRPM() { + + + } + + public void run() { + + //Read the network table from vision + 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 * Config.METER_PER_FOOT ; + + // adjuste the distance for the shooter + adjustedDistanceToOutPortInMeters = distanceToOuterPortInMeters + Config.D_CAMERA_SHOOTER_IN_METERS; + + //Calculate the RPM of the shooter wheel. + double targetV = initVelocity( adjustedDistanceToOutPortInMeters); + targetRPM = velocityToRPM (targetV); + } + + // provide feedback to the shuffleboard for Driver Team + // vision assisted calculated RPM + SmartDashboard.putNumber("Vision RPM", targetRPM); + } + + public static 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 +public static 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/config/Config.java b/src/main/java/frc/robot/config/Config.java index f387650..63c7f5f 100644 --- a/src/main/java/frc/robot/config/Config.java +++ b/src/main/java/frc/robot/config/Config.java @@ -144,10 +144,19 @@ private Config() { public static String VISION_TABLE_NAME_POWERCELL = "MergeVisionPi20"; public static String VISION_TABLE_NAME_OUTERPORT = "MergeVisionPipelinePi21"; 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"; + // 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.037, 0.0, 0.0, 0.018d, 0.0, 0.25); public static double DRIVETRAIN_D_SPECIFIC = robotSpecific(0.0023, 0.0, 0.0, 0.0016d, 0.0, 0.03);