diff --git a/SwerveCode/src/main/java/frc/robot/Collector.java b/SwerveCode/src/main/java/frc/robot/Collector.java index 6376ab5..c245eb3 100644 --- a/SwerveCode/src/main/java/frc/robot/Collector.java +++ b/SwerveCode/src/main/java/frc/robot/Collector.java @@ -3,52 +3,68 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Collector { + private DigitalInput lineSensorTop; private DigitalInput lineSensorBottom; WPI_TalonFX processing; WPI_TalonFX staging; + WPI_TalonFX collectorArm; public enum CollectorState{ONE_BALL_BOTTOM, ONE_BALL_TOP, TWO_BALLS, NO_BALLS_LOADED} private CollectorState collectorState; //This class will contain all new collector specific methods public Collector(){ - WPI_TalonFX processing = new WPI_TalonFX(Constants.newFlywheelCollector); - WPI_TalonFX staging = new WPI_TalonFX(Constants.newFlywheelStaging); + collectorArm = new WPI_TalonFX(Constants.newCollectorArm); + processing = new WPI_TalonFX(Constants.newFlywheelCollector); + staging = new WPI_TalonFX(Constants.newFlywheelStaging); lineSensorBottom = new DigitalInput(Constants.LINE_BREAK_BOTTOM_SENSOR_PORT); lineSensorTop = new DigitalInput(Constants.LINE_BREAK_TOP_SENSOR_PORT); + + processing.setInverted(true); + staging.setInverted(true); + } + + public void raiseCollectorArm(){ + this.collectorArm.set(-0.); + } + + public void lowerCollectorArm(){ + this.collectorArm.set(0.05); } //Drives the processing wheels for state machine - public void driveProcessingWheels(){ - processing.set(0.5); + public void driveProcessingWheels(double speed){ + this.processing.set(speed); } //Drives staging wheel for state machine - public void driveStagingWheels(){ - processing.set(0.5); + public void driveStagingWheels(double speed){ + this.staging.set(speed); } //Stops processing wheels for state machine public void stopProcessingWheels(){ - processing.set(0); + this.processing.set(0); } + //Stops Staging wheels for state machine public void stopStagingWheels(){ - processing.set(0); + this.staging.set(0); } //Manually reverses the Staging wheels public void reverseStagingWheels(){ - staging.set(-1); + this.staging.set(-0.2); } //Manually reverses the Processing wheels public void reverseProcessingWheels(){ - processing.set(-1); + this.processing.set(-0.2); } //Stops entire intake system if needed @@ -59,8 +75,8 @@ public void intakeAllStop(){ //This runs the intake system in its entirety if needed public void intakeAllRun(){ - this.driveProcessingWheels(); - this.driveStagingWheels(); + this.driveProcessingWheels(0.2); + this.driveStagingWheels(0.2); } //reverses intake entirely in the event we need it @@ -71,18 +87,19 @@ public void intakeReverse(){ public CollectorState determineCollectorState(){ boolean topState = lineSensorTop.get(); + SmartDashboard.getBoolean("LineSensorTop", topState); boolean bottomState = lineSensorBottom.get(); - + SmartDashboard.getBoolean("LineSensorBottom", bottomState); //this makes sure to tell which states are which - if(bottomState){ - if(topState){ + if(!bottomState){ + if(!topState){ collectorState = CollectorState.TWO_BALLS; }else{ collectorState = CollectorState.ONE_BALL_BOTTOM; } }else{ - if(topState){ + if(!topState){ collectorState = CollectorState.ONE_BALL_TOP; }else{ collectorState = CollectorState.NO_BALLS_LOADED; @@ -90,32 +107,48 @@ public CollectorState determineCollectorState(){ } + + SmartDashboard.putString("Collector State", collectorState.toString()); return collectorState; } public void ballControl(){ + //this is a simple state machine controlling what ball wheels run and when switch(this.determineCollectorState()){ case NO_BALLS_LOADED: //when there are no balls loaded we want to run the processing wheels to collect 1 ball - this.driveProcessingWheels(); + this.driveProcessingWheels(0.2); + this.driveStagingWheels(0.2); + this.lowerCollectorArm(); break; case ONE_BALL_BOTTOM: //when there is one ball at the bottom we want to stop the wheels pushing it in and start driving the middle wheels - this.stopProcessingWheels(); - this.driveStagingWheels(); + this.driveProcessingWheels(0.2); + this.driveStagingWheels(0.2); break; - + case ONE_BALL_TOP: //when theres one ball at the top we want to make sure that the staging wheels don't move the ball and run the bottom wheels to collect another this.stopStagingWheels(); - this.driveProcessingWheels(); + this.driveProcessingWheels(0.2); break; case TWO_BALLS: //when we have 2 balls we don't want to run any of the intake modules this.intakeAllStop(); + this.raiseCollectorArm(); break; - } + + + } + + + + SmartDashboard.putBoolean("LineSensorTop", lineSensorTop.get()); + SmartDashboard.putBoolean("LineSensorBottom", lineSensorBottom.get()); } + public void manualControl(){ + + } } diff --git a/SwerveCode/src/main/java/frc/robot/ColorSensor.java b/SwerveCode/src/main/java/frc/robot/ColorSensor.java index d390cc2..494b1a5 100644 --- a/SwerveCode/src/main/java/frc/robot/ColorSensor.java +++ b/SwerveCode/src/main/java/frc/robot/ColorSensor.java @@ -1,5 +1,5 @@ //code by Zolton -package frc.robot; +/*package frc.robot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.I2C; @@ -78,4 +78,5 @@ public String toString(){ } return s; } -} \ No newline at end of file +} +*/ \ No newline at end of file diff --git a/SwerveCode/src/main/java/frc/robot/Constants.java b/SwerveCode/src/main/java/frc/robot/Constants.java index 0e52c75..4920b5f 100644 --- a/SwerveCode/src/main/java/frc/robot/Constants.java +++ b/SwerveCode/src/main/java/frc/robot/Constants.java @@ -58,16 +58,37 @@ public final class Constants { // // CAN IDs for the ball collector and launching system // - public static final int newFlywheelLeft = 22; + public static final int newFlywheelLeft = 23; public static final int newFlywheelRight = 20; - public static final int newFlywheelStaging = 21; - public static final int newFlywheelCollector = 23; + public static final int newFlywheelStaging = 22; + public static final int newFlywheelCollector = 21; + public static final int newCollectorArm = 24; //Change this I don't know what the arm CAN is //IR sensor ports - public static final int LINE_BREAK_TOP_SENSOR_PORT = -2; - public static final int LINE_BREAK_BOTTOM_SENSOR_PORT = -1; + public static final int LINE_BREAK_TOP_SENSOR_PORT = 0; + public static final int LINE_BREAK_BOTTOM_SENSOR_PORT = 1; public static enum ALLIANCE_COLOR { BLUE, RED, NONE } + + //ringVision constants + public static double RING_LOCK_ERROR = 1.0; + public static double TURRET_ERROR = 0.5; // Allowed aiming error in degrees + public static double RING_CAMERA_HEIGHT = 36.0; // Limelight height above ground (inches) + public static double RING_CAMERA_ANGLE = 0.0; // Limelight camera angle above horizontal (degrees) + public static double RING_TARGET_HEIGHT = 104; // Center of target above ground (inches) + public static double TURRET_ROTATE_KP = 1.0; // 15.6 Proportional constant for turret rotate speed + public static String LIMELIGHT_RING = "limelight-ring"; + + //ballVision constants + public static double BALL_LOCK_ERROR = 2.0; + public static double BALL_ERROR = 0.5; // Allowed aiming error in degrees + public static double BALL_CAMERA_HEIGHT = 0.0; + public static double BALL_CAMERA_ANGLE = 0.0; + public static double BALL_TARGET_HEIGHT = 0.0; + public static double BALL_ROTATE_KP = 7.0; // Proportional constant for turret rotate speed + public static String LIMELIGHT_BALL = "limelight-ball"; + public static double MIN_TURN_SPEED = 0.75; + public static double MAX_TURN_SPEED = 0.5; } diff --git a/SwerveCode/src/main/java/frc/robot/Robot.java b/SwerveCode/src/main/java/frc/robot/Robot.java index 608ba51..375f0a9 100644 --- a/SwerveCode/src/main/java/frc/robot/Robot.java +++ b/SwerveCode/src/main/java/frc/robot/Robot.java @@ -13,7 +13,8 @@ import edu.wpi.first.wpilibj.XboxController; //this puts in the xbox contoller stuff import frc.robot.Shooter; import frc.robot.Constants.ALLIANCE_COLOR; - +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.math.geometry.Rotation2d; /** * The VM is configured to automatically run this class, and to call the * functions corresponding to @@ -34,12 +35,12 @@ public class Robot extends TimedRobot { public Drivetrain drive; public Autonomous autonomous; - public Vision vision; - public ballTargeting ball; + public Vision visionRing; + public Vision visionBall; public Locality locality; public Shooter shooter; public Collector collector; - public ColorSensor color; + // public ColorSensor color; /** @@ -54,14 +55,13 @@ public void robotInit() { driverController = new XboxController(0); shooterController = new XboxController(1); drive = new Drivetrain(); - vision = new Vision(); - ball = new ballTargeting(); + visionRing = new Vision(Constants.LIMELIGHT_RING, Constants.RING_LOCK_ERROR, Constants.RING_CAMERA_HEIGHT, Constants.RING_CAMERA_ANGLE, Constants.RING_TARGET_HEIGHT, Constants.TURRET_ROTATE_KP); + visionBall = new Vision(Constants.LIMELIGHT_BALL, Constants.BALL_LOCK_ERROR, Constants.BALL_CAMERA_HEIGHT, Constants.BALL_CAMERA_ANGLE, Constants.BALL_TARGET_HEIGHT, Constants.BALL_ROTATE_KP); locality = new Locality(0, 0); shooter = new Shooter(); color = new ColorSensor(); - - ball.setLimelightAllianceColor(ALLIANCE_COLOR.RED); - + //ball.setLimelightAllianceColor(ALLIANCE_COLOR.RED); + NetworkTableInstance.getDefault().getTable("limelight-ball").getEntry("pipeline").setNumber(1); } /** @@ -125,40 +125,42 @@ public void teleopPeriodic(){ double translateY; double rotate; - locality.updatePosition(drive.getYaw(), vision); + visionBall.updateVision(); + visionRing.updateVision(); + + locality.updatePosition(drive.getYaw(), visionRing); // Read gamepad controls rotate = (driverController.getRightX() * Drivetrain.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND) * ConfigRun.ROTATE_POWER; // Right joystick translateX = (driverController.getLeftY() * Drivetrain.MAX_VELOCITY_METERS_PER_SECOND) * ConfigRun.TRANSLATE_POWER; //X is forward Direction, Forward on Joystick is Y translateY = (driverController.getLeftX() * Drivetrain.MAX_VELOCITY_METERS_PER_SECOND) * ConfigRun.TRANSLATE_POWER; - shooter.testshooter(shooterController); - ball.ballAim(); + shooter.testshooter(shooterController); if(driverController.getRightBumper() == true){ - drive.drive(ChassisSpeeds.fromFieldRelativeSpeeds(-joystickDeadband(translateX),-joystickDeadband(translateY), -vision.autoAim() , drive.getGyroscopeRotation())); //Inverted due to Robot Directions being the opposite of controller directions + drive.drive(ChassisSpeeds.fromFieldRelativeSpeeds(-joystickDeadband(translateX), + -joystickDeadband(translateY), -visionRing.turnRobot() , drive.getGyroscopeRotation())); //Inverted due to Robot Directions being the opposite of controller directions } else if(driverController.getLeftBumper() == true) { - drive.drive(ChassisSpeeds.fromFieldRelativeSpeeds(0,0, -ball.ballAim() , drive.getGyroscopeRotation())); + drive.drive(ChassisSpeeds.fromFieldRelativeSpeeds(visionBall.moveTowardsTarget(), 0, visionBall.turnRobot(), + Rotation2d.fromDegrees(0))); } else { - drive.drive(ChassisSpeeds.fromFieldRelativeSpeeds(-joystickDeadband(translateX),-joystickDeadband(translateY), -joystickDeadband(rotate), drive.getGyroscopeRotation())); //Inverted due to Robot Directions being the opposite of controller directions + drive.drive(ChassisSpeeds.fromFieldRelativeSpeeds(-joystickDeadband(translateX), + -joystickDeadband(translateY), -joystickDeadband(rotate), drive.getGyroscopeRotation())); //Inverted due to Robot Directions being the opposite of controller directions } - //this makes sure that when the driver pushes the A button they can control the shooter directly, if not this runs the ball control - if(shooterController.getAButton()){ - shooter.testshooter(shooterController); - }else{ - collector.ballControl(); - } + + shooter.collectorDriverControl(shooterController); + SmartDashboard.putNumber("Rotate", rotate); - color.getColors(); + /*color.getColors(); if(color.getProximity() > 200){ color.updateCurrentBallColor(); - } - + { + */ } /** This function is called once when the robot is disabled. */ diff --git a/SwerveCode/src/main/java/frc/robot/Shooter.java b/SwerveCode/src/main/java/frc/robot/Shooter.java index 6d25bec..72e47b7 100644 --- a/SwerveCode/src/main/java/frc/robot/Shooter.java +++ b/SwerveCode/src/main/java/frc/robot/Shooter.java @@ -1,12 +1,16 @@ package frc.robot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Collector.CollectorState; import edu.wpi.first.wpilibj.XboxController; //this puts in the xbox contoller stuff import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; +import java.util.stream.Collectors; + import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice; @@ -17,51 +21,83 @@ public class Shooter{ - //constants - private static final int LineBreakSensorPort = 0; + //constants\ + Collector collector; //motor controllors - public WPI_TalonFX collector; + public WPI_TalonFX processing; public WPI_TalonFX flyWheelLeft; public WPI_TalonFX flyWheelRight; public WPI_TalonFX staging; - - //motor groups - MotorControllerGroup flyWheel; - // Line break sensor - private DigitalInput lineSensorA; + public enum ShooterState{AUTONOMOUS,SHOOT} + private ShooterState shooterState; + + double flywheelVelocity; + public Shooter(){ - collector = new WPI_TalonFX(Constants.newFlywheelCollector); + collector = new Collector(); + processing = new WPI_TalonFX(Constants.newFlywheelCollector); flyWheelLeft = new WPI_TalonFX(Constants.newFlywheelLeft); flyWheelRight = new WPI_TalonFX(Constants.newFlywheelRight); staging = new WPI_TalonFX(Constants.newFlywheelStaging); - flyWheel = new MotorControllerGroup(flyWheelLeft, flyWheelRight); - lineSensorA = new DigitalInput(LineBreakSensorPort); + flyWheelLeft.follow(flyWheelRight); + flyWheelLeft.setInverted(InvertType.OpposeMaster); - flyWheelLeft.setInverted(true); - collector.setInverted(true); - } + flyWheelRight.setInverted(false); + processing.setInverted(true); + staging.setInverted(true); - public void testshooter(XboxController shooterController) { + flywheelVelocity = SmartDashboard.getNumber("enter velocity", 10); + SmartDashboard.putNumber("Flywheel Velocity", flywheelVelocity); - // Line break sensor + flywheelVelocity = -0.5; + flyWheelRight.set(ControlMode.PercentOutput, flywheelVelocity); + } - SmartDashboard.putBoolean("Line Sensor", lineSensorA.get()); + public ShooterState determineShooterState(){ + CollectorState collectorState = collector.determineCollectorState(); - double flyWheelSpeed; + if(collectorState != CollectorState.NO_BALLS_LOADED){ + shooterState = ShooterState.SHOOT; + }else{ + shooterState = ShooterState.AUTONOMOUS; + } - //link flywheel motors + SmartDashboard.putString("State", shooterState.toString()); + return shooterState; + } - //control speed of the flywheel - flyWheelSpeed = shooterController.getRightTriggerAxis(); - collector.set(shooterController.getLeftTriggerAxis()); - staging.set(shooterController.getLeftTriggerAxis()); - - flyWheel.set(flyWheelSpeed); + public void collectorDriverControl(XboxController shootController){ + + switch (determineShooterState()) { + case SHOOT: + if(shootController.getBButton()){ + manualControl(); + + }else{ + collector.ballControl(); + } + break; + + case AUTONOMOUS: + collector.ballControl(); + break; + } } + + + public void manualControl(){ + collector.driveStagingWheels(1); + collector.driveProcessingWheels(1); + SmartDashboard.putNumber("Running Manual", 1); + } +} + + + diff --git a/SwerveCode/src/main/java/frc/robot/Vision.java b/SwerveCode/src/main/java/frc/robot/Vision.java index 6463053..87646f3 100644 --- a/SwerveCode/src/main/java/frc/robot/Vision.java +++ b/SwerveCode/src/main/java/frc/robot/Vision.java @@ -1,209 +1,238 @@ package frc.robot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.XboxController; //this puts in the xbox contoller stuff -import edu.wpi.first.wpilibj.GenericHID; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants.ALLIANCE_COLOR; -public class Vision { - //constantse - private static double TURRET_ERROR = 0.5; // Allowed aiming error in degrees - private static double LOCK_ERROR = 1.0; - private static double TURRET_ROTATE_KP = 15.6; // Proportional constant for turret rotate speed - private static double RPM_TO_TICKS_MS = 2048.0/600.0; // Conversion factor for rotational velocity - private static double TRIGGER_MOTOR_SPEED = 0.4; // Maximum power for the motor feeding the flywheel - private static double SHOOTING_RPM_RANGE = 20; // Allowed RPM error for flywheel - //distance - private static double CAMERA_HEIGHT = 59.0; // Limelight height above ground (inches) - private static double CAMERA_ANGLE = 0.0; // Limelight camera angle above horizontal (degrees) - private static double TARGET_HEIGHT = 104; // Center of target above ground (inches) - private static double TARGET_HEIGHT_DELTA = TARGET_HEIGHT - CAMERA_HEIGHT; - // - private static double targetHeight = 104; - private double targetAngle; - private double targetDistance; - private static double cameraAngle = 0; - private static double cameraHeight = 1; - // - private static double MANUAL_POWER = 0.5; // Turret power for manual control - // - private static double TURRET_TICKS_PER_DEGREE = 1770 / 90; // Encoder ticks per degree +import java.util.LinkedList; - // Motor controllers - public WPI_TalonSRX turretRotate; // Motor for rotating the turret +public class Vision { + + //constants passed in during initilization + private double lockError; + private double cameraHeight; + private double cameraAngle; + private double targetHeight; + private double rotationKP; // Network Table entries - private NetworkTableEntry tx; // Angle error (x) from LimeLight camera - private NetworkTableEntry ty; // Angle error (y) from LimeLight camera - private NetworkTableEntry ta; // Target area measurement from LimeLight camera - private NetworkTableEntry tv; // Target valid indicator from Limelight camera + private NetworkTableEntry tx; // Angle error (x) from LimeLight camera + private NetworkTableEntry ty; // Angle error (y) from LimeLight camera + private NetworkTableEntry ta; // Target area measurement from LimeLight camera + private NetworkTableEntry tv; // Target valid indicator from Limelight camera // Shared variables public boolean targetValid; // Indicate when the Limelight camera has found a target public boolean targetLocked; // Indicate when the turret is centered on the target public double targetRange; // Range from robot to target (inches) public Timer timer; + private double processedDx = 0; + private double processedDy = 0; //Private autoaim variables - private double turretSpeed; - private double xError; - private double yError; - private double area; - private double turret_rotate_kd = 0; - private double lastTime = 0; - private double xtime = 0; + private double turnSpeed; + private double lastTime = 0; + private double xtime = 0; private double lastAngle = 0; private double changeInAngleError = 0; - public static boolean autonomousEnabled; - private final double DEG_TO_RAD = 0.0174533; - private final double IN_TO_METERS = 0.0254; + //constants for averaging limelight averages + private int MIN_LOCKS = 3; + private int STAT_SIZE = 5; + + private LinkedList previousCoordinates; + + private String limelightName; + + // Pipeline constants + private static int BLUE_PIPELINE = 1; + private static int RED_PIPELINE = 0; + + private final double DEG_TO_RAD = 0.0174533; + private final double IN_TO_METERS = 0.0254; + /** - * This constructor will intialize the motors and internal variables for the robot turret + * This constructor will intialize internal variables for the robot turret */ - public Vision() { + public Vision(String limelightName, double lockError, double cameraHeight, + double cameraAngle, double targetHeight, double rotationKP) { - //set up networktables for limelight - NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-ring"); + // Set up networktables for limelight + NetworkTable table = NetworkTableInstance.getDefault().getTable(limelightName); tx = table.getEntry("tx"); ty = table.getEntry("ty"); ta = table.getEntry("ta"); tv = table.getEntry("tv"); + previousCoordinates = new LinkedList(); + // Establish initial values for variables we share targetValid = false; targetLocked = false; targetRange = 0.0; timer = new Timer(); timer.start(); + + this.limelightName = limelightName; + this.lockError = lockError; + this.cameraHeight = cameraHeight; + this.cameraAngle = cameraAngle; + this.targetHeight = targetHeight; + this.rotationKP = rotationKP; + } + + + public void updateVision(){ //method should be called continuously during autonomous and teleop + double xError; + double yError; + double area; + double totalDx = 0; + double totalDy = 0; + int totalValid = 0; + + // Read the Limelight data from the Network Tables + xError = tx.getDouble(0.0); + yError = ty.getDouble(0.0); + area = ta.getDouble(0.0); + targetValid = (tv.getDouble(0.0) != 0); // Convert the double output to boolean + + //generates average of limelight parameters + previousCoordinates.add(new LimelightData(xError, yError, targetValid)); + if (previousCoordinates.size() > STAT_SIZE){ + previousCoordinates.removeFirst(); + } + + for(LimelightData data: previousCoordinates){ + if (data.ballValid == true){ + totalDx = data.dx + totalDx; + totalDy = data.dy + totalDy; + totalValid = totalValid +1; + } + } + + processedDx = totalDx/totalValid; + processedDy = totalDy/totalValid; + targetValid = (totalValid >= MIN_LOCKS); + + targetRange = distanceToTarget(); + + if (Math.abs(processedDx) < lockError){ // Turret is pointing at target (or no target) + targetLocked = targetValid; // We are only locked when targetValid + } + else{ + targetLocked = false; + } + + //post driver data to smart dashboard periodically + SmartDashboard.putNumber(limelightName + "/xerror in radians", Math.toRadians(xError)); + SmartDashboard.putNumber(limelightName + "/LimelightX", xError); + SmartDashboard.putNumber(limelightName + "/LimelightY", yError); + SmartDashboard.putNumber(limelightName + "/LimelightArea", area); + SmartDashboard.putBoolean(limelightName + "/Target Valid", targetValid); + SmartDashboard.putNumber(limelightName + "/Change in Angle Error", changeInAngleError); + SmartDashboard.putNumber(limelightName + "/Average Y", processedDy); + SmartDashboard.putNumber(limelightName + "/Average X", processedDx); + SmartDashboard.putNumber(limelightName + "/Total Valid", totalValid); + SmartDashboard.putNumber(limelightName + "/Target Range", targetRange); + SmartDashboard.putBoolean(limelightName + "/Target Locked", targetLocked); } + public double delta(){ //gets the change in angle over time(seconds) xtime = timer.get(); - changeInAngleError = (xError - lastAngle)/(xtime - lastTime); - lastAngle = xError; // reset initial angle - lastTime = xtime; // reset initial time + changeInAngleError = (processedDx - lastAngle)/(xtime - lastTime); + lastAngle = processedDx; // reset initial angle + lastTime = xtime; // reset initial time return changeInAngleError; } - /** - * Converts the RPM input to the ticks/100ms velocity metric used by the - * Falcon 500 motor - * - * @param rpm Rotational velocity in Revolutions Per Minute (RPM) - * @return Encoder ticks per 100msec - */ - public double rpmToFalcon(double rpm){ - return rpm * RPM_TO_TICKS_MS; - } - - /** - * Converts the internal Falcon 500 velocity measurement of ticks per 100msec - * into Revolutions Per Minute (RPM) - * @param falcon Rotational velocity in ticks/100msec. This is usually read from the controller - * @return Rotational velocity in RPM - */ - public double falconToRPM(double falcon){ - return falcon / RPM_TO_TICKS_MS; - } - /** - * Read the target x error from the Limelight camera and move the turret until the error is 0 - * - * @param ballShooterController Used to enable autoAim turret motion - */ /** * Gives distance from the robot to the target in meters - * + * Compute range to target. + * Formula taken from https://docs.limelightvision.io/en/latest/cs_estimating_distance.html * @return distance in meters */ public double distanceToTarget(){ - targetValid = (tv.getDouble(0.0) != 0); // Convert the double output to boolean - double targetAngle = ty.getDouble(0.0); if (targetValid){ - double distanceInches = TARGET_HEIGHT_DELTA / Math.tan((cameraAngle + targetAngle) * DEG_TO_RAD);//Equation is from limelight documentation finding distance + double distanceInches = (targetHeight - cameraHeight) / Math.tan((cameraAngle + processedDy) * DEG_TO_RAD);//Equation is from limelight documentation finding distance return distanceInches * IN_TO_METERS; } return -1; } + + /** * * * @return angle offset in radians */ public double offsetAngle(){ - double offsetAngle = tx.getDouble(0.0); - - return Math.toRadians(offsetAngle); + return Math.toRadians(processedDx); } - public double autoAim() { - - // Read the Limelight data from the Network Tables - xError = tx.getDouble(0.0); - yError = ty.getDouble(0.0); - area = ta.getDouble(0.0); - targetValid = (tv.getDouble(0.0) != 0); // Convert the double output to boolean + public double turnRobot(){ if (targetValid){ - // Compute range to target. - // Formula taken from https://docs.limelightvision.io/en/latest/cs_estimating_distance.html - targetRange = TARGET_HEIGHT_DELTA / Math.tan(Math.toRadians(CAMERA_ANGLE + yError)); - // Setting power based on the xError causes the turret to slow down as the error approaches 0 // This prevents the turret from overshooting 0 and oscillating back and forth // KP is a scaling factor that we tested - turretSpeed = Math.toRadians(xError) * TURRET_ROTATE_KP + turret_rotate_kd*delta(); - turretSpeed = Math.max(turretSpeed, -4); - turretSpeed = Math.min(turretSpeed, 4); - } - else if(lastAngle != 0){ - turretSpeed = Math.toRadians(lastAngle) * TURRET_ROTATE_KP; - turretSpeed = Math.max(turretSpeed, -4); - turretSpeed = Math.min(turretSpeed, 4); + turnSpeed = Math.toRadians(processedDx) * rotationKP; // + turret_rotate_kd*delta(); + turnSpeed = Math.max(turnSpeed, -4); + turnSpeed = Math.min(turnSpeed, 4); + + // + // Set a minimum turnSpeed so that we don't get stuck when close to zero error + // + if (turnSpeed > 0){ + turnSpeed = Math.max(turnSpeed, Constants.MIN_TURN_SPEED); + } + else{ + turnSpeed = Math.min(turnSpeed, -Constants.MIN_TURN_SPEED); + } } else{ - turretSpeed = 1; + turnSpeed = 1; // Spin in a circle until a target is located } - if (Math.abs(xError) < LOCK_ERROR) { // Turret is pointing at target (or no target) - targetLocked = targetValid; // We are only locked when targetValid - } - else{ - targetLocked = false; + // + // Stop turning immediately when target is locked (at center) + // + if (targetLocked){ + turnSpeed = 0; } - + SmartDashboard.putNumber(limelightName + "/Turret Speed", turnSpeed); - //post driver data to smart dashboard periodically - SmartDashboard.putNumber("xerror in radians", Math.toRadians(xError)); - SmartDashboard.putNumber("LimelightX", xError); - SmartDashboard.putNumber("LimelightY", yError); - SmartDashboard.putNumber("LimelightArea", area); - SmartDashboard.putNumber("Target Range", targetRange); - SmartDashboard.putBoolean("Target Valid", targetValid); - SmartDashboard.putBoolean("Target Locked", targetLocked); - SmartDashboard.putNumber("Turret Speed", turretSpeed); - SmartDashboard.putNumber("Change in Angle Error", changeInAngleError); - - //x = 0 when the camera sees the target is in the center - // Only allow the turret to track when commanded - if (Math.abs(xError) < TURRET_ERROR) { // Turret is pointing at target (or no target) - return 0.0; //Returns 0 if the turret is within the margin error + return -turnSpeed; + } + + // + // Drive towards the target + // This should probably be updated to base speed on distance from the target + // + public double moveTowardsTarget(){ + double moveSpeed = 0.0; + + if (targetLocked == true){ + moveSpeed = -.5; } - else { - return turretSpeed; + SmartDashboard.putNumber(limelightName + "/Move Speed", moveSpeed); + return moveSpeed; + } + + + private class LimelightData{ + double dx; + double dy; + boolean ballValid; + + public LimelightData(double xError, double yError, boolean ballValid){ + dx = xError; + dy = yError; + this.ballValid = ballValid; } - } - - } \ No newline at end of file diff --git a/SwerveCode/src/main/java/frc/robot/ballTargeting.java b/SwerveCode/src/main/java/frc/robot/ballTargeting.java index af858a0..e7ac04b 100644 --- a/SwerveCode/src/main/java/frc/robot/ballTargeting.java +++ b/SwerveCode/src/main/java/frc/robot/ballTargeting.java @@ -52,7 +52,7 @@ public class ballTargeting{ double translateY = (Drivetrain.MAX_VELOCITY_METERS_PER_SECOND) / 2; public Drivetrain drive; - public ColorSensor colorSensor; + //public ColorSensor colorSensor; //motor controllors public WPI_TalonFX collector; @@ -136,7 +136,7 @@ public void moveTowardsBall(){ } - public void setLimelightAllianceColor(ALLIANCE_COLOR color){ + /* public void setLimelightAllianceColor(ALLIANCE_COLOR color){ if (colorSensor.getCurrentBallColor() == ALLIANCE_COLOR.RED){ NetworkTableInstance.getDefault().getTable("limelight ball").getEntry("pipeline").setNumber(RED_PIPELINE); } @@ -144,7 +144,7 @@ public void setLimelightAllianceColor(ALLIANCE_COLOR color){ NetworkTableInstance.getDefault().getTable("limelight ball").getEntry("pipeline").setNumber(BLUE_PIPELINE); } } - +*/ }