Public Pastes
SHARE
TWEET

FRC 4787 problems

a guest Jan 31st, 2014 34 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /*----------------------------------------------------------------------------*/
  2. /* Copyright (c) FIRST 2008. All Rights Reserved.                             */
  3. /* Open Source Software - may be modified and shared by FRC teams. The code   */
  4. /* must be accompanied by the FIRST BSD license file in the root directory of */
  5. /* the project.                                                               */
  6. /*----------------------------------------------------------------------------*/
  7. package edu.wpi.first.wpilibj.templates;
  8.  
  9. import edu.wpi.first.wpilibj.*;
  10. import java.lang.Math;
  11. import edu.wpi.first.wpilibj.CounterBase.EncodingType;
  12. import edu.wpi.first.wpilibj.camera.AxisCamera;
  13. import edu.wpi.first.wpilibj.camera.AxisCameraException;
  14. import edu.wpi.first.wpilibj.image.BinaryImage;
  15. import edu.wpi.first.wpilibj.image.ColorImage;
  16. import edu.wpi.first.wpilibj.image.CriteriaCollection;
  17. import edu.wpi.first.wpilibj.image.NIVision;
  18. import edu.wpi.first.wpilibj.image.NIVisionException;
  19. import edu.wpi.first.wpilibj.image.ParticleAnalysisReport;
  20. import edu.wpi.first.wpilibj.image.RGBImage;
  21. import edu.wpi.first.wpilibj.networktables.NetworkTable;
  22. import java.util.Vector;
  23.  
  24. /**
  25.  * The VM is configured to automatically run this class, and to call the
  26.  * functions corresponding to each mode, as described in the SimpleRobot
  27.  * documentation. If you change the name of this class or the package after
  28.  * creating this project, you must also update the manifest file in the resource
  29.  * directory.
  30.  */
  31. public class Walle extends SimpleRobot {
  32.  
  33.     // Variable initializations
  34.     RobotDrive drive;
  35.     Joystick leftStick;
  36.     Joystick rightStick;
  37.  
  38.     double CAMERA_ANGLE = 49;
  39.  
  40.     // Joystick constants
  41.     int LEFT_JOYSTICK_USB = 1;
  42.     int RIGHT_JOYSTICK_USB = 2;
  43.  
  44.     // Motor constants
  45.     int FRONT_LEFT_MOTOR_PWM = 1;
  46.     int REAR_LEFT_MOTOR_PWM = 2;
  47.     int FRONT_RIGHT_MOTOR_PWM = 3;
  48.     int REAR_RIGHT_MOTOR_PWM = 4;
  49.     int LAUNCH_PWM;
  50.     int RIGHT_WINCH_PWM;
  51.     int LEFT_WINCH_PWM;
  52.     int LINE_ACTUATOR_PWM;
  53.  
  54.     // Motor controller configurations
  55.     Jaguar frontLeft = new Jaguar(FRONT_LEFT_MOTOR_PWM);
  56.     Jaguar rearLeft = new Jaguar(REAR_LEFT_MOTOR_PWM);
  57.     Jaguar frontRight = new Jaguar(FRONT_RIGHT_MOTOR_PWM);
  58.     Jaguar rearRight = new Jaguar(REAR_RIGHT_MOTOR_PWM);
  59.     Jaguar launch = new Jaguar(LAUNCH_PWM);
  60.     Jaguar rightWinch = new Jaguar(RIGHT_WINCH_PWM);
  61.     Jaguar leftWinch = new Jaguar(LEFT_WINCH_PWM);
  62.  
  63.     Encoder encoder = new Encoder(1, 2, true, EncodingType.k4X);
  64.  
  65.     // Camera constants for distance calculation
  66.     int IMAGE_RESOLUTION = 480;
  67.     int VIEW_ANGLE = 49;
  68.  
  69.     /* Limits for the rectangularity and
  70.      *  aspect ratio scores, anything below
  71.      *  this will not be considered as a
  72.      *  target. */
  73.     int RECTANGULARAITY_LIMIT = 40;
  74.     int ASPECT_RATIO_LIMIT = 55;
  75.  
  76.     int TAPE_WIDTH_LIMIT = 50;
  77.     int VERTICAL_SCORE_LIMIT = 50;
  78.     int LR_SCORE_LIMIT = 50;
  79.     int AREA_MINIMUM = 150;
  80.     int MAX_PARTICLES = 8;
  81.  
  82.     AxisCamera camera;
  83.     CriteriaCollection criteria;
  84.  
  85.     public class Scores {
  86.  
  87.         double rectangularity;
  88.         double aspectRatioVertical;
  89.         double aspectRatioHorizontal;
  90.     }
  91.  
  92.     /* This object contain data about the target
  93.      *  including the scores, the position, and
  94.      *  whether or not it is hot. */
  95.     public class TargetReport {
  96.  
  97.         int verticalPos;
  98.         int horizontalPos;
  99.         boolean hot;
  100.         double totalScore;
  101.         double leftScore;
  102.         double rightScore;
  103.         double tapeWidthScore;
  104.         double verticalScore;
  105.     }
  106.  
  107.     public Walle() {
  108.         drive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
  109.         leftStick = new Joystick(LEFT_JOYSTICK_USB);
  110.         rightStick = new Joystick(RIGHT_JOYSTICK_USB);
  111.     }
  112.  
  113.     public void robotInit() {
  114.         camera = AxisCamera.getInstance();
  115.         criteria = new CriteriaCollection();
  116.         criteria.addCriteria(NIVision.MeasurementType.IMAQ_MT_AREA, AREA_MINIMUM, 65535, false);
  117.     }
  118.  
  119.     /* This function converts a ratio from the
  120.      *  range of 0 - 1.0 into a number from 1-100.*/
  121.     public double ratioToScore(double ratio) {
  122.         return (Math.max(0, Math.min(100 * (1 - Math.abs(1 - ratio)), 100)));
  123.     }
  124.  
  125.     public double rectangularity(ParticleAnalysisReport report) {
  126.         return (report.boundingRectWidth * report.boundingRectHeight != 0)
  127.                 ? (100 * (report.particleArea / (report.boundingRectWidth * report.boundingRectHeight))) : 0.0;
  128.     }
  129.  
  130.     public double aspectRatioScore(BinaryImage image, ParticleAnalysisReport report, boolean vertical, int particle) throws NIVisionException {
  131.         double rectLong, rectShort, idealAspectRatio, aspectRatio;
  132.         // The dimensions of the vertical reflector strip
  133.         // are 4.0 X 32 and the dimensions of the horizontal
  134.         // reflector is 23.5 X 4 used in the game.
  135.         idealAspectRatio = vertical ? (4.0 / 32) : (23.5 / 4);
  136.         // Returns the number of particles on the longer
  137.         // side and the shorter side of the rectangle
  138.         rectLong = NIVision.MeasureParticle(image.image, particle, false, NIVision.MeasurementType.IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE);
  139.         rectShort = NIVision.MeasureParticle(image.image, particle, false, NIVision.MeasurementType.IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE);
  140.         // If the image is wider than it is long, then calculate the
  141.         // aspect ratio by dividing the longer value by the shorter
  142.         // and the shorter by the longer otherwise
  143.         if (report.boundingRectWidth > report.boundingRectHeight) {
  144.             aspectRatio = ratioToScore((rectLong / rectShort) / idealAspectRatio);
  145.         } else {
  146.             aspectRatio = ratioToScore((rectShort / rectLong) / idealAspectRatio);
  147.         }
  148.         return aspectRatio;
  149.     }
  150.  
  151.     /* This function determines if the score is a target by determining
  152.      *  if it is a rectangle, anything with a rectuanguliry lower than
  153.      *  the limit is not considered a rectangle, then the function measures
  154.      *  aspect ratio because... */
  155.     public boolean compareScores(Scores score, boolean vertical) {
  156.         boolean isTarget = true;
  157.         isTarget &= score.rectangularity > RECTANGULARAITY_LIMIT;
  158.         if (vertical) {
  159.             isTarget &= score.aspectRatioVertical > ASPECT_RATIO_LIMIT;
  160.         } else {
  161.             isTarget &= score.aspectRatioHorizontal > ASPECT_RATIO_LIMIT;
  162.         }
  163.         return isTarget;
  164.     }
  165.  
  166.     /* This function computes the distance away from the camera given
  167.      *  the image. It uses the reso*/
  168.     public double computeDistance(BinaryImage image, ParticleAnalysisReport report, int particle) throws NIVisionException {
  169.         double rectLong, height;
  170.         int targetHeight = 32;
  171.  
  172.         rectLong = NIVision.MeasureParticle(image.image, particle, false, NIVision.MeasurementType.IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE);
  173.         height = Math.min(report.boundingRectHeight, rectLong);
  174.  
  175.         return IMAGE_RESOLUTION * targetHeight / (height * 12 * 2 * Math.tan(VIEW_ANGLE * Math.PI / (180 / 2)));
  176.     }
  177.    
  178.     public boolean hotOrNot(TargetReport target)
  179.  {
  180.   boolean isHot = true;
  181.  
  182.   isHot &= target.tapeWidthScore >= TAPE_WIDTH_LIMIT;
  183.   isHot &= target.verticalScore >= VERTICAL_SCORE_LIMIT;
  184.   isHot &= (target.leftScore > LR_SCORE_LIMIT) | (target.rightScore > LR_SCORE_LIMIT);
  185.  
  186.   return isHot;
  187.  }
  188.    
  189.     public void setWinchAngle(double angle) {
  190.         int winchAngle = (int)angle;
  191.     }
  192.  
  193.     /**
  194.      * This function is called once each time the robot enters autonomous mode.
  195.      */
  196.     public void autonomous() {
  197.         TargetReport target = new TargetReport();
  198.         int verticalTargets[] = new int[MAX_PARTICLES];
  199.         int horizontalTargets[] = new int[MAX_PARTICLES];
  200.         int verticalTargetCount, horizontalTargetCount;
  201.         while (isAutonomous() && isEnabled()) {
  202.             try {
  203.                 ColorImage image = camera.getImage();
  204.                 BinaryImage thresholdImage = image.thresholdHSV(105, 137, 230, 255, 133, 183);
  205.                 BinaryImage filteredImage = thresholdImage.particleFilter(criteria);
  206.                 Scores scores[] = new Scores[filteredImage.getNumberParticles()];
  207.                 horizontalTargetCount = verticalTargetCount = 0;
  208.                 if (filteredImage.getNumberParticles() > 0) {
  209.                     for (int i = 0; i < MAX_PARTICLES && i < filteredImage.getNumberParticles(); i++) {
  210.                         ParticleAnalysisReport report = filteredImage.getParticleAnalysisReport(i);
  211.                         scores[i] = new Scores();
  212.                         scores[i].rectangularity = rectangularity(report);
  213.                         scores[i].aspectRatioVertical = aspectRatioScore(filteredImage, report, true, i);
  214.                         scores[i].aspectRatioHorizontal = aspectRatioScore(filteredImage, report, false, i);
  215.  
  216.                         if (compareScores(scores[i], false)) {
  217.                             System.out.println("particle: " + i + "is a Horizontal Target centerX: " + report.center_mass_x + "centerY: " + report.center_mass_y);
  218.                             horizontalTargets[horizontalTargetCount++] = i;
  219.                         } else if (compareScores(scores[i], true)) {
  220.                             System.out.println("particle: " + i + "is a Vertical Target centerX: " + report.center_mass_x + "centerY: " + report.center_mass_y);
  221.                             verticalTargets[verticalTargetCount++] = i;
  222.                         } else {
  223.                             System.out.println("particle: " + i + "is not a Target centerX: " + report.center_mass_x + "centerY: " + report.center_mass_y);
  224.                         }
  225.                         System.out.println("rect: " + scores[i].rectangularity + "ARHoriz: " + scores[i].aspectRatioHorizontal);
  226.                         System.out.println("ARVert: " + scores[i].aspectRatioVertical);
  227.                     }
  228.                     target.totalScore = target.leftScore = target.rightScore = target.tapeWidthScore = target.verticalScore = 0;
  229.                     target.verticalPos = verticalTargets[0];
  230.                     for (int i = 0; i < verticalTargetCount; i++) {
  231.                         ParticleAnalysisReport verticalReport = filteredImage.getParticleAnalysisReport(verticalTargets[i]);
  232.                         for (int j = 0; j < horizontalTargetCount; j++) {
  233.                             ParticleAnalysisReport horizontalReport = filteredImage.getParticleAnalysisReport(horizontalTargets[j]);
  234.                             double horizWidth, horizHeight, vertWidth, leftScore, rightScore, tapeWidthScore, verticalScore, total;
  235.  
  236.                             horizWidth = NIVision.MeasureParticle(filteredImage.image, horizontalTargets[j], false, NIVision.MeasurementType.IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE);
  237.                             vertWidth = NIVision.MeasureParticle(filteredImage.image, verticalTargets[i], false, NIVision.MeasurementType.IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE);
  238.                             horizHeight = NIVision.MeasureParticle(filteredImage.image, horizontalTargets[j], false, NIVision.MeasurementType.IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE);
  239.  
  240.                             //Determine if the horizontal target is in the expected location to the left of the vertical target
  241.                             leftScore = ratioToScore(1.2 * (verticalReport.boundingRectLeft - horizontalReport.center_mass_x) / horizWidth);
  242.                             //Determine if the horizontal target is in the expected location to the right of the  vertical target
  243.                             rightScore = ratioToScore(1.2 * (horizontalReport.center_mass_x - verticalReport.boundingRectLeft - verticalReport.boundingRectWidth) / horizWidth);
  244.                             //Determine if the width of the tape on the two targets appears to be the same
  245.                             tapeWidthScore = ratioToScore(vertWidth / horizHeight);
  246.                             //Determine if the vertical location of the horizontal target appears to be correct
  247.                             verticalScore = ratioToScore(1 - (verticalReport.boundingRectTop - horizontalReport.center_mass_y) / (4 * horizHeight));
  248.                             total = leftScore > rightScore ? leftScore : rightScore;
  249.                             total += tapeWidthScore + verticalScore;
  250.  
  251.                             //If the target is the best detected so far store the information about it
  252.                             if (total > target.totalScore) {
  253.                                 target.horizontalPos = horizontalTargets[j];
  254.                                 target.verticalPos = verticalTargets[i];
  255.                                 target.totalScore = total;
  256.                                 target.leftScore = leftScore;
  257.                                 target.rightScore = rightScore;
  258.                                 target.tapeWidthScore = tapeWidthScore;
  259.                                 target.verticalScore = verticalScore;
  260.                             }
  261.                         }
  262.                         //Determine if the best target is a Hot target
  263.                         target.hot = hotOrNot(target);
  264.  
  265.                     }
  266.                     if (verticalTargetCount > 0) {
  267.                         ParticleAnalysisReport distanceReport = filteredImage.getParticleAnalysisReport(target.verticalPos);
  268.                         double distance = computeDistance(filteredImage, distanceReport, target.verticalPos);
  269.                         if (target.hot) {
  270.                             System.out.println("Hot target located");
  271.                             System.out.println("Distance: " + distance);
  272.                             double targetHeight = Math.tan(VIEW_ANGLE) * distance;
  273.                         } else {
  274.                             System.out.println("No hot target present");
  275.                             System.out.println("Distance: " + distance);
  276.                         }
  277.                     }
  278.                 }
  279.                 filteredImage.free();
  280.                 thresholdImage.free();
  281.                 image.free();
  282.             } catch (AxisCameraException ex) {
  283.                 ex.printStackTrace();
  284.             } catch (NIVisionException ex) {
  285.                 ex.printStackTrace();
  286.             }
  287.         }
  288.  
  289.     }
  290.  
  291.     /**
  292.      * This function is called once each time the robot enters operator control.
  293.      */
  294.     public void operatorControl() {
  295.         while (true && isOperatorControl() && isEnabled()) {
  296.             drive.tankDrive(leftStick.getY(), leftStick.getX());
  297.             if (rightStick.getRawButton(3)) {
  298.                
  299.             }
  300.             Timer.delay(0.005);
  301.         }
  302.     }
  303.  
  304.     /**
  305.      * This function is called once each time the robot enters test mode.
  306.      */
  307.     public void test() {
  308.     }
  309. }
RAW Paste Data
Top