/*----------------------------------------------------------------------------*/ /* Copyright (c) FIRST 2008. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ /*----------------------------------------------------------------------------*/ package edu.wpi.first.wpilibj.samples; //TODO: switch to using alternate blocking function call with specific task //TODO: fix PCVideo server failure killing crio //TODO: Tune loop better //TODO: add more joystick functionality import edu.wpi.first.wpilibj.camera.*; import edu.wpi.first.wpilibj.*; import edu.wpi.first.wpilibj.image.*; /** * The VM is configured to automatically run this class, and to call the * functions corresponding to each mode, as described in the IterativeRobot * documentation. If you change the name of this class or the package after * creating this project, you must also update the manifest file in the resource * directory. */ public class CircleTrackerDemo extends IterativeRobot { private Jaguar oneMotor = new Jaguar(9); private Jaguar twoMotor = new Jaguar (10); private Jaguar ballcontrol = new Jaguar(5); private Jaguar winch = new Jaguar(6); private Jaguar kicker = new Jaguar(7); private Joystick leftJoystick = new Joystick(1); private Joystick rightJoystick = new Joystick(2); private Joystick armJoystick = new Joystick(3); private Watchdog wd = this.getWatchdog(); private AxisCamera camera = AxisCamera.getInstance(); private ColorImage image; private Solenoid shifthigh = new Solenoid(1); private Solenoid shiftlow = new Solenoid(2); private Compressor compressor = new Compressor(1,1); double kScoreThreshold = .01; AxisCamera cam; Gyro gyro = new Gyro(1); RobotDrive drive = new RobotDrive(1, 2); { drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true); drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true); } Joystick js = new Joystick(1); PIDController turnController = new PIDController(.08, 0.0, 0.5, gyro, new PIDOutput() { public void pidWrite(double output) { drive.arcadeDrive(0, output); } }, .005); TrackerDashboard trackerDashboard = new TrackerDashboard(); /** * This function is run when the robot is first started up and should be * used for any initialization code. */ public void robotInit() { Timer.delay(10.0); cam = AxisCamera.getInstance(); cam.writeResolution(AxisCamera.ResolutionT.k320x240); cam.writeBrightness(0); gyro.setSensitivity(.007); turnController.setInputRange(-360.0, 360.0); turnController.setTolerance(1 / 90. * 100); turnController.disable(); } /** * This function is called periodically during autonomous */ public void autonomousPeriodic() { } /** * This function is called while the robot is disabled. */ public void disabledPeriodic() { } /** * This function is called at the beginning of teleop */ public void teleopInit() { } boolean lastTrigger = false; /** * This function is called periodically during operator control */ public void teleopPeriodic() { compressor.start(); camera.notify(); camera.freshImage(); camera.getInstance(); camera.writeBrightness(0); camera.writeResolution(AxisCamera.ResolutionT.k640x480); long startTime = Timer.getUsClock(); while(true) { if(leftJoystick.getRawButton(1) && rightJoystick.getRawButton(1)) { oneMotor.set(rightJoystick.getY()); twoMotor.set(leftJoystick.getY()); } else if(leftJoystick.getRawButton(1)) { oneMotor.set(-leftJoystick.getY() - leftJoystick.getX()); twoMotor.set(leftJoystick.getY() - leftJoystick.getX()); } else { oneMotor.set(0); twoMotor.set(0); } if (leftJoystick.getRawButton(2) || armJoystick.getRawButton(2)) { kicker.set(100); } else { kicker.set(0); } if(leftJoystick.getRawButton(11)) { ballcontrol.set(-100); } else { ballcontrol.set(0); } if(armJoystick.getRawButton(6)) { ballcontrol.set(-100); } else { ballcontrol.set(0); } if (leftJoystick.getRawButton(10)) { ballcontrol.set(100); } else { ballcontrol.set(0); } if(armJoystick.getRawButton(7)) { ballcontrol.set(100); } else { ballcontrol.set(0); } if (leftJoystick.getRawButton(4)) { shiftlow.set(true); } else { shiftlow.set(false); } if (leftJoystick.getRawButton(5)) { shifthigh.set(true); } else { shifthigh.set(false); } if (armJoystick.getRawButton(3)) { winch.set(100); } else { winch.set(0); } if (!js.getTrigger()) { if (lastTrigger) turnController.disable(); lastTrigger = false; drive.arcadeDrive(js); } else { if (!lastTrigger) { turnController.enable(); turnController.setSetpoint(gyro.pidGet()); } lastTrigger = true; try { if (cam.freshImage()) {// && turnController.onTarget()) { double gyroAngle = gyro.pidGet(); ColorImage image = cam.getImage(); Thread.yield(); Target[] targets = Target.findCircularTargets(image); Thread.yield(); image.free(); if (targets.length == 0 || targets[0].m_score < kScoreThreshold) { System.out.println("No target found"); Target[] newTargets = new Target[targets.length + 1]; newTargets[0] = new Target(); newTargets[0].m_majorRadius = 0; newTargets[0].m_minorRadius = 0; newTargets[0].m_score = 0; for (int i = 0; i < targets.length; i++) { newTargets[i + 1] = targets[i]; } trackerDashboard.updateVisionDashboard(0.0, gyro.getAngle(), 0.0, 0.0, newTargets); } else { System.out.println(targets[0]); System.out.println("Target Angle: " + targets[0].getHorizontalAngle()); turnController.setSetpoint(gyroAngle + targets[0].getHorizontalAngle()); trackerDashboard.updateVisionDashboard(0.0, gyro.getAngle(), 0.0, targets[0].m_xPos / targets[0].m_xMax, targets); } } } catch (NIVisionException ex) { ex.printStackTrace(); } catch (AxisCameraException ex) { ex.printStackTrace(); } System.out.println("Time : " + (Timer.getUsClock() - startTime) / 1000000.0); System.out.println("Gyro Angle: " + gyro.getAngle()); } wd.feed(); } } }