Examples with Block org.usfirst.frc948.NRGRobot2018.vision.PixyCam.Block used on opensource projects

Example 1 with Block

use of org.usfirst.frc948.NRGRobot2018.vision.PixyCam.Block in project NRGRobot2018 by NRG948.

the class Robot method robotPeriodic.

@Override
public void robotPeriodic() {
    positionTracker.updatePosition();
    ArrayList<Block> currFrame = RobotMap.pixy.getPixyFrameData();
    if (currFrame.size() > 0) {
        Block cube = currFrame.get(0);
        SmartDashboard.putString("Cube/Cube", cube.toString());
        SmartDashboard.putNumber("Cube/Angle to turn", CubeCalculations.getAngleToTurn(cube));
        SmartDashboard.putNumber("Cube/Inches to cube from width", CubeCalculations.getDistanceFromWidth(cube));
    }
    SmartDashboard.putNumber("navx gyro yaw", RobotMap.navx.getYaw());
    SmartDashboard.putNumber("PositionTracker/current x", positionTracker.getX());
    SmartDashboard.putNumber("PositionTracker/current y", positionTracker.getY());
    SmartDashboard.putNumber("Encoders/omni x", RobotMap.xEncoder.getDistance());
    SmartDashboard.putNumber("Encoders/omni y", RobotMap.yEncoder.getDistance());
    SmartDashboard.putNumber("Encoders/left front", RobotMap.leftFrontEncoder.getDistance());
    SmartDashboard.putNumber("Encoders/left rear", RobotMap.leftRearEncoder.getDistance());
    SmartDashboard.putNumber("Encoders/right front", RobotMap.rightFrontEncoder.getDistance());
    SmartDashboard.putNumber("Encoders/right rear", RobotMap.rightRearEncoder.getDistance());
    SmartDashboard.putNumber("Encoders/lifter", RobotMap.cubeLiftEncoder.getDistance());
    SmartDashboard.putNumber("Encoders/tilter", RobotMap.cubeTiltEncoder.getDistance());
    SmartDashboard.putNumber("Encoders/MechY", Robot.positionTracker.getMechY());
    SmartDashboard.putData("LifterLimitSwitches/upper", RobotMap.lifterUpperLimitSwitch);
    SmartDashboard.putData("LifterLimitSwitches/lower", RobotMap.lifterLowerLimitSwitch);
    SmartDashboard.putNumber("LeftJoystick/x", OI.getLeftJoystickX());
    SmartDashboard.putNumber("LeftJoystick/y", OI.getLeftJoystickY());
    SmartDashboard.putNumber("RightJoystick/x", OI.getRightJoystickX());
    SmartDashboard.putNumber("RightJoystick/y", OI.getRightJoystickY());
    SmartDashboard.putNumber("RightJoystick/rot", OI.getRightJoystickRot());
    SmartDashboard.putNumber("XBox/left joystick y", OI.getXBoxLeftY());
    SmartDashboard.putNumber("XBox/right joystick y", OI.getXBoxRightY());
    SmartDashboard.putNumber("XBox/left trigger", OI.getXBoxTriggerL());
    SmartDashboard.putNumber("XBox/right trigger", OI.getXBoxTriggerR());
    SmartDashboard.putNumber("XBox/d-pad", OI.xboxController.getPOV());
}

Example 2 with Block

use of org.usfirst.frc948.NRGRobot2018.vision.PixyCam.Block in project NRGRobot2018 by NRG948.

the class DriveToCube method execute.

// Called repeatedly when this Command is scheduled to run
protected void execute() {
    ArrayList<Block> currFrame = RobotMap.pixy.getPixyFrameData();
    Block currBlock;
    SmartDashboard.putNumber("DriveToCube/current frame size", currFrame.size());
    if (currFrame.size() > 0) {
        System.out.println(currFrame.size());
        currBlock = currFrame.get(0);
        distanceToCube = CubeCalculations.getDistanceFromWidth(currBlock);
        double cubeNormalized = CubeCalculations.getDistanceToCenterNormalized(currBlock);
        turnPower = MathUtil.clampNegativePositive(cubeNormalized, 0.15, 0.7);
        Robot.drive.rawDriveCartesian(0, drivePower, turnPower);
        if (driveUntilCubeAcquired) {
            drivePower = MathUtil.clamp(Math.abs(distanceToCube / DISTANCE_TO_SLOW), 0.2, 0.5);
        } else {
            drivePower = Math.min(1.0, (distanceToCube - DISTANCE_TO_STOP) / (DISTANCE_TO_SLOW - DISTANCE_TO_STOP));
        }
        SmartDashboard.putNumber("DriveToCube/distance", distanceToCube);
        System.out.println("Driving" + " drivePower " + drivePower + " turnPower " + turnPower + " distanceToCube " + distanceToCube);
    }
}

Example 3 with Block

use of org.usfirst.frc948.NRGRobot2018.vision.PixyCam.Block in project NRGRobot2018 by NRG948.

the class StrafeAlignWithCube method execute.

// Called repeatedly when this Command is scheduled to run
protected void execute() {
    ArrayList<Block> currentFrame = RobotMap.pixy.getPixyFrameData();
    SmartDashboard.putNumber("CenterToCube/number of objects", currentFrame.size());
    if (currentFrame.size() > 0) {
        Block currentBlock = currentFrame.get(0);
        alignError = CubeCalculations.getDistanceToCenterNormalized(currentBlock);
        // minimum strafe power is .5 to prevent stalling out
        double strafePower = /*MathUtil.clampNegativePositive(alignError, 0.5, 1.0)*/
        Math.copySign(CONSTANT_STRAFE_POWER, alignError);
        double calculatedTurnPower = Robot.drive.turnPIDControllerExecute(RobotMap.gyro.getAngle());
        Robot.drive.rawDriveCartesian(strafePower, 0, calculatedTurnPower);
        SmartDashboard.putNumber("CenterToCube/strafe power", strafePower);
    }
}