/*----------------------------------------------------------------------------*/
/* 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();
}
}
}