|
Re: Camera Tracking help please
We got a somewhat tracking code working today, but i would like the people of chief delphi to take a look at it. I've posed errors in the code (as comments) that my mentor and I can't figure out. PLZ HELP!
package edu.wpi.first.wpilibj.templates;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.camera.AxisCamera;
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;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ParticleAnalysisReport ;
public class RobotTemplate extends IterativeRobot {
Joystick leftStick = new Joystick(1);
Joystick rightStick = new Joystick(2);
Joystick joypad = new Joystick(3);
RobotDrive tank = new RobotDrive(3,1,4,2);
Victor shooter = new Victor(5);
Relay ballLift = new Relay(1);
Relay ballHolder = new Relay(2);
Relay shooterAngle = new Relay(3);
AxisCamera camera = AxisCamera.getInstance();
CriteriaCollection cc;
public int centerup = 172;
public int centerdown = 152;
DigitalInput shootermin = new DigitalInput(1);
DigitalInput shootermax = new DigitalInput(2);
public void robotInit() {
cc = new CriteriaCollection();
cc.addCriteria(NIVision.MeasurementType.IMAQ_MT_AR EA, 30, 400, false);
cc.addCriteria(NIVision.MeasurementType.IMAQ_MT_AR EA, 40, 400, false);
}
/**
* This function is called periodically during autonomous
*/
public void autonomousPeriodic() {
//
}
/**
* This function is called periodically during operator control
*/
public void teleopPeriodic() {
tank.tankDrive(leftStick, rightStick);
if(joypad.getRawButton(5)){
ballLift.set(Relay.Value.kForward);
ballHolder.set(Relay.Value.kForward);
}else if(joypad.getRawButton(7)){
ballLift.set(Relay.Value.kReverse);
ballHolder.set(Relay.Value.kReverse);
}else{
ballLift.set(Relay.Value.kOff);
ballHolder.set(Relay.Value.kOff);
}
if(joypad.getRawButton(4)){
shooter.set(1);
}else if(joypad.getRawButton(3)){
shooter.set(-1);
}else{
shooter.set(0);
}
if(joypad.getRawButton(6)){
increaseShooterAngle();
}else if(joypad.getRawButton(8)){
decreaseShooterAngle();
}else{
shooterAngle.set(Relay.Value.kOff);
}
if(joypad.getRawButton(2)){
System.out.println("Button 2 Pressed");
centerShooter();
}
if(joypad.getRawButton(1)){
readLimitswiches();
}
}
/**
* This function is called periodically during test mode
*/
public void testPeriodic() {
}
private void centerShooter() {
ColorImage image = null;
BinaryImage thresholdImage = null;
BinaryImage bigObjectsImage = null;
BinaryImage convexHullImage = null;
BinaryImage filteredImage = null;
try{
image = camera.getImage();
thresholdImage = image.thresholdRGB(0, 45, 25, 225, 0, 45);
bigObjectsImage = thresholdImage.removeSmallObjects(false, 2);
convexHullImage = bigObjectsImage.convexHull(false);
filteredImage = convexHullImage.particleFilter(cc);
ParticleAnalysisReport[] reports = filteredImage.getOrderedParticleAnalysisReports();
for(int i = 0; i <reports.length +1;i++){
System.out.println(reports[i].center_mass_y);
if(reports[i].center_mass_y>centerdown){
increaseShooterAngle();
Timer.delay(.1);
shooterAngle.set(Relay.Value.kOff);
}else if(reports[i].center_mass_y<centerup){
decreaseShooterAngle();
Timer.delay(.1);
shooterAngle.set(Relay.Value.kOff);
}else{
System.out.println("Your Good");
}
}
}catch(Exception ex){
System.out.println("Failed");
System.out.println(ex);
//java.lang.ArrayIndexOutOfBoundsException: on [edu.wpi.first.wpilibj.image.ParticleAnalysisReport of length 0 with index 0
}finally{
}
try{
filteredImage.free();
convexHullImage.free();
bigObjectsImage.free();
thresholdImage.free();
image.free();
}catch(NIVisionException ex){
}
}
private void increaseShooterAngle() {
if(!shootermax.get()){
shooterAngle.set(Relay.Value.kForward);
}else{
shooterAngle.set(Relay.Value.kOff);
System.out.println("At Maximum Angle");
}
}
private void decreaseShooterAngle() {
if(!shootermin.get()){
shooterAngle.set(Relay.Value.kReverse);
}else{
shooterAngle.set(Relay.Value.kOff);
System.out.println("At Minnimum Angle");
}
}
private void readLimitswiches() {
if(shootermin.get()){
System.out.println("Shooter Min is true");
}else{
System.out.println("Shooter Min is False");
}
}
}
Last edited by 2472Programer : 01-12-2013 at 03:47 PM.
Reason: changing post
|