Examples with PositionTracker org.usfirst.frc948.NRGRobot2018.utilities.PositionTracker used on opensource projects

Example 1 with PositionTracker

use of org.usfirst.frc948.NRGRobot2018.utilities.PositionTracker 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 PositionTracker

use of org.usfirst.frc948.NRGRobot2018.utilities.PositionTracker in project NRGRobot2018 by NRG948.

the class Robot method robotInit.

/**
 * This function is run when the robot is first started up and should be used for any
 * initialization code.
 */
@Override
public void robotInit() {
    System.out.println("robotInit() started");
    preferences = Preferences.getInstance();
    RobotMap.init();
    drive = new Drive();
    cubeAcquirer = new CubeAcquirer();
    cubeLifter = new CubeLifter();
    cubeTilter = new CubeTilter();
    climber = new Climber();
    positionTracker = new PositionTracker(0, 0);
    cubeSolenoid = new CubeSolenoid();
    OI.init();
    OI.initTriggers();
    // OI must be constructed after subsystems. If the OI creates Commands
    // (which it very likely will), subsystems are not guaranteed to be
    // constructed yet. Thus, their requires() statements may grab null
    // pointers. Bad news. Don't move it.
    initPreferences();
    RobotMap.pixy.startVisionThread();
    RobotMap.arduino.startArduinoThread();
    autoPositionChooser = new SendableChooser<AutoStartingPosition>();
    autoPositionChooser.addDefault("Left", AutoStartingPosition.LEFT);
    autoPositionChooser.addObject("Center", AutoStartingPosition.CENTER);
    autoPositionChooser.addObject("Right", AutoStartingPosition.RIGHT);
    autoMovementChooser = new SendableChooser<AutoMovement>();
    autoMovementChooser.addObject("Switch", AutoMovement.SWITCH);
    autoMovementChooser.addObject("Scale", AutoMovement.SCALE);
    autoMovementChooser.addDefault("Both", AutoMovement.BOTH);
    autoMovementChooser.addDefault("Forward", AutoMovement.FORWARD);
    autoMovementChooser.addDefault("None", AutoMovement.NONE);
    SmartDashboard.putData("Choose autonomous position", autoPositionChooser);
    SmartDashboard.putData("Choose autonomous movement", autoMovementChooser);
    System.out.println("robotInit() done");
}