/*
* To change this template, choose Tools | Templates
* and open the template in the editor.
*/
package edu.wpi.first.wpilibj.templates.subsystems;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.image.BinaryImage;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.CriteriaCollection;
import edu.wpi.first.wpilibj.image.NIVision.MeasurementType;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ParticleAnalysisReport;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.OI;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.MecanumDriveWithJoystick;
import edu.wpi.first.wpilibj.templates.commands.TankDriveWithJoystick;
/**
*
* @author Eddie
*/
public class DriveTrain extends Subsystem {
// Put methods for controlling this subsystem
// here. Call these from Commands.
private Jaguar
LEFT_FRONT_MOTOR = new Jaguar(RobotMap.DRIVE_FRONT_LEFT),
LEFT_REAR_MOTOR = new Jaguar(RobotMap.DRIVE_REAR_LEFT),
RIGHT_FRONT_MOTOR = new Jaguar(RobotMap.DRIVE_FRONT_RIGHT),
RIGHT_REAR_MOTOR = new Jaguar(RobotMap.DRIVE_REAR_RIGHT);
private RobotDrive drive;
public Encoder enc_FrontLeft;
public Encoder enc_FrontRight;
public Encoder enc_RearLeft;
public Encoder enc_RearRight;
public DigitalInput EitherOne;
AxisCamera camera;
CriteriaCollection cc;
double tan23 = 0.424474816;
public DriveTrain() {
super("DriveTrain");
drive = new RobotDrive(LEFT_FRONT_MOTOR, LEFT_REAR_MOTOR, RIGHT_FRONT_MOTOR, RIGHT_REAR_MOTOR);
drive.setSafetyEnabled(false);
if(RobotMap.DRIVE_FRONT_LEFT_REVERSED) {
drive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true);
}
if(RobotMap.DRIVE_FRONT_RIGHT_REVERSED) {
drive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
}
if(RobotMap.DRIVE_REAR_LEFT_REVERSED) {
drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
}
if(RobotMap.DRIVE_REAR_RIGHT_REVERSED) {
drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
}
camera = AxisCamera.getInstance("10.32.60.11");
cc = new CriteriaCollection();
cc.addCriteria(MeasurementType.IMAQ_MT_BOUNDING_RECT_WIDTH, 40, 400, false);
cc.addCriteria(MeasurementType.IMAQ_MT_BOUNDING_RECT_HEIGHT, 55, 400, false);
enc_FrontLeft = new Encoder(RobotMap.ENC_FRONT_LEFTA, RobotMap.ENC_FRONT_LEFTB);
enc_FrontRight = new Encoder(RobotMap.ENC_FRONT_RIGHTA, RobotMap.ENC_FRONT_RIGHTB);
enc_RearLeft = new Encoder(RobotMap.ENC_REAR_LEFTA, RobotMap.ENC_REAR_LEFTB);
enc_RearRight = new Encoder(RobotMap.ENC_REAR_RIGHTA, RobotMap.ENC_REAR_RIGHTB);
enc_FrontLeft.start();
enc_FrontRight.start();
enc_RearLeft.start();
enc_RearRight.start();
EitherOne = new DigitalInput(12);
}
public void atSpeed(double speed) {
//drive.drive(speed, 0.0);
drive.tankDrive(speed, speed);
}
public void strafeAtSpeed(double speed, double direction) {
drive.mecanumDrive_Polar(0.4, direction, 0.0);
}
public void toTank() {
setDefaultCommand(new TankDriveWithJoystick());
}
public void toMecanum() {
setDefaultCommand(new MecanumDriveWithJoystick());
}
protected void initDefaultCommand() {
setDefaultCommand(new MecanumDriveWithJoystick());
EitherOne.get();
}
public void mecanumDriveWithJoystick() {
drive.mecanumDrive_Polar(OI.getInstance().getDriveJoystick().getMagnitude(), OI.getInstance().getDriveJoystick().getDirectionDegrees(), OI.getInstance().getDriveJoystick2().getX());
}
public void tankDriveWithJoystick() {
double left = -OI.getInstance().getShooterJoystick().getY();
double right = OI.getInstance().getDriveJoystick().getY();
drive.tankDrive(left, right);
}
public void changeMaxDriveSpeed(double change) {
drive.setMaxOutput(change);
}
public void toDashboardEncoders() {
SmartDashboard.putInt("Front Left", enc_FrontLeft.getRaw());
SmartDashboard.putInt("Front Right", enc_FrontRight.getRaw());
SmartDashboard.putInt("Rear Left", enc_RearLeft.getRaw());
SmartDashboard.putInt("Rear Right", enc_RearRight.getRaw());
}
public void proportional(double desired, double current) {
double offset = current;
double error = desired - offset;
double Kp = 0.02;
SmartDashboard.putDouble("Kp", Kp);
double[] array = {0, current};
double output = 0.0;
SmartDashboard.putDouble("Error", error);
while(Math.abs(desired - offset) > 1) {
Kp = SmartDashboard.getDouble("Kp", 0.01);
array = findCameraDistance();
offset = array[1];
error = desired - current;
output = (Kp * error);
SmartDashboard.putDouble("Error", error);
if(output >= 0.5)
{
output = 0.5;
}
if(output <= -0.5)
{
output = -0.5;
}
drive.tankDrive(output, -output);
}
}
public double[] findCameraDistance() {
double distance = 0.0;
double offset = 0.0;
double[] array = {0, 0};
try {
ColorImage image = null;
try {
image = camera.getImage();
} catch (AxisCameraException ex) {
} catch (NIVisionException ex) {
}
BinaryImage thresholdImage = image.thresholdHSL(0, 255, 0, 31, 229, 255); // keep only red objects
BinaryImage bigObjectsImage = thresholdImage.removeSmallObjects(false, 2); // remove small artifacts
BinaryImage convexHullImage = bigObjectsImage.convexHull(false); // fill in occluded rectangles
BinaryImage filteredImage = convexHullImage.particleFilter(cc); // find filled in rectangles
ParticleAnalysisReport[] reports = filteredImage.getOrderedParticleAnalysisReports(); // get list of results
if(reports[1] != null) {
ParticleAnalysisReport r = reports[1];
System.out.println("x: " + r.center_mass_x_normalized + " y: " + r.center_mass_y_normalized);
System.out.println(r.boundingRectHeight + " " + r.boundingRectWidth);
distance = (9/tan23) * 240/r.boundingRectHeight;
System.out.println("Distance: " + distance);
SmartDashboard.putDouble("Distance", distance);
array[0] = distance;
offset = (46 * (160 - r.center_mass_x)) / 320;
System.out.println("Offset: " + offset);
SmartDashboard.putDouble("Offset", offset);
array[1] = offset;
}
System.out.println(filteredImage.getNumberParticles() + " " + Timer.getFPGATimestamp());
filteredImage.free();
convexHullImage.free();
bigObjectsImage.free();
thresholdImage.free();
image.free();
} catch (NIVisionException ex) {
}
return array;
}
public void updateStatus() {
SmartDashboard.putDouble("Drive - Front Left", LEFT_FRONT_MOTOR.getSpeed());
SmartDashboard.putDouble("Drive - Rear Left", LEFT_REAR_MOTOR.getSpeed());
SmartDashboard.putDouble("Drive - Front Right", RIGHT_FRONT_MOTOR.getSpeed());
SmartDashboard.putDouble("Drive - Rear Right", RIGHT_REAR_MOTOR.getSpeed());
//SmartDashboard.putDouble("Camera Tilt Angle", getCameraTiltAngle());
//SmartDashboard.putDouble("Camera Pan Angle", getCameraPanAngle());
}
}