diff --git a/Steamworks/.classpath b/Steamworks/.classpath index 5ee687e..8b450a7 100644 --- a/Steamworks/.classpath +++ b/Steamworks/.classpath @@ -8,6 +8,5 @@ - diff --git a/Steamworks/src/org/usfirst/frc/team4786/robot/Robot.java b/Steamworks/src/org/usfirst/frc/team4786/robot/Robot.java index a31c4bb..2b16b39 100644 --- a/Steamworks/src/org/usfirst/frc/team4786/robot/Robot.java +++ b/Steamworks/src/org/usfirst/frc/team4786/robot/Robot.java @@ -1,13 +1,13 @@ package org.usfirst.frc.team4786.robot; import org.usfirst.frc.team4786.robot.commands.DoNothing; -import org.usfirst.frc.team4786.robot.commands.DriveArcSpeed; import org.usfirst.frc.team4786.robot.commands.DriveToLeftGearPeg; import org.usfirst.frc.team4786.robot.commands.DriveToPosition; import org.usfirst.frc.team4786.robot.commands.DriveToRightGearPeg; import org.usfirst.frc.team4786.robot.commands.OpenLoopDrive; +import org.usfirst.frc.team4786.robot.commands.ProcessVisionContinuously; import org.usfirst.frc.team4786.robot.commands.SwitchFrontSide; -import org.usfirst.frc.team4786.robot.commands.TurnToAngle; +import org.usfirst.frc.team4786.robot.commands.VisionAlignWithPeg; import org.usfirst.frc.team4786.robot.subsystems.Arduino; import org.usfirst.frc.team4786.robot.subsystems.Climber; import org.usfirst.frc.team4786.robot.subsystems.DrawBridge; @@ -52,6 +52,7 @@ public class Robot extends IterativeRobot { public static CvSink cvSink; public static CvSource outputStream; public static CvSource regStream; + public static CvSource visionStream; public static UsbCamera gearPlacementCamera; public static UsbCamera ballPlacementCamera; @@ -74,7 +75,7 @@ public class Robot extends IterativeRobot { @Override public void robotInit() { - + System.out.println("ERROR: RUSSIAN HACKING ATTEMPT DETECTED"); gearPlacementCamera = CameraServer.getInstance().startAutomaticCapture("Gear", 0); ballPlacementCamera = CameraServer.getInstance().startAutomaticCapture("Ball Camera", 1); gearPlacementCamera.setFPS(15); @@ -82,7 +83,12 @@ public void robotInit() { gearPlacementCamera.setResolution(RobotMap.cameraFOVWidth,RobotMap.cameraFOVHeight); ballPlacementCamera.setResolution(RobotMap.cameraFOVWidth, RobotMap.cameraFOVHeight); - cvSink = CameraServer.getInstance().getVideo(); + gearPlacementCamera.setExposureManual(1); + + cvSink = CameraServer.getInstance().getVideo("Gear"); + //outputStream = CameraServer.getInstance().putVideo("Gear Camera", RobotMap.cameraFOVWidth, RobotMap.cameraFOVHeight); + //= new CvSource("GearCamera", VideoMode.PixelFormat.kRGB565, RobotMap.cameraFOVWidth, RobotMap.cameraFOVHeight, 15); + frontSide = "Gear"; @@ -92,16 +98,14 @@ public void robotInit() { sendableChooser = new SendableChooser(); - sendableChooser.addDefault("Drive to Center Gear Peg", new DriveToPosition(7)); + sendableChooser.addDefault("Drive to Center Gear Peg", new DriveToPosition(4.83)); sendableChooser.addObject("Do Nothing!", new DoNothing()); sendableChooser.addObject("Drive to Baseline", new DriveToPosition(10)); sendableChooser.addObject("Drive to Left Gear Peg", new DriveToLeftGearPeg()); sendableChooser.addObject("Drive to Right Gear Peg", new DriveToRightGearPeg()); - //sendableChooser.addObject("GetToGearTest", new GearFromOffset()); + //sendableChooser.addObject("Vision align with peg", new VisionAlignWithPeg()); SmartDashboard.putData("Autonomous Selector", sendableChooser); - /*Command initial = new SwitchFrontSide(); - initial.start();*/ } @Override public void disabledInit() { @@ -131,6 +135,7 @@ public void disabledPeriodic() { @Override public void autonomousInit() { + alliance = DriverStation.getInstance().getAlliance(); //send correct alliance data to arduino @@ -147,8 +152,11 @@ public void autonomousInit() { SmartDashboard.putString("Alliance", allianceColorVal); - //camera.setExposureManual(RobotMap.exposure); - autonomousCommand = (Command) sendableChooser.getSelected(); + //camera.setExposureManual(RobotMap.exposure); + autonomousCommand = (Command) sendableChooser.getSelected(); + //Command visionProcess = new ProcessVisionContinuously(); + //visionProcess.start(); + if (autonomousCommand != null) autonomousCommand.start(); } @@ -166,16 +174,17 @@ public void autonomousInit() { @Override public void autonomousPeriodic() { Scheduler.getInstance().run(); + SmartDashboard.putNumber("Left Encoder Positon", driveTrain.getLeftEncoderPosition()); SmartDashboard.putNumber("Right Encoder Positon", driveTrain.getRightEncoderPosition()); - SmartDashboard.putNumber("Servo Angle", drawBridge.getServoAngle()); } @Override public void teleopInit() { - - //gearPlacementCamera.setExposureAuto(); + System.out.println("ERROR: RUSSIAN HACKING ATTEMPT DETECTED"); + + gearPlacementCamera.setExposureAuto(); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to @@ -185,6 +194,9 @@ public void teleopInit() { if (autonomousCommand != null) autonomousCommand.cancel(); + Command switchFront = new SwitchFrontSide(); + switchFront.start(); + teleopCommand = new OpenLoopDrive(); if(teleopCommand != null) @@ -205,7 +217,7 @@ public void teleopPeriodic() { SmartDashboard.putBoolean("Peg Present", gear.pegLimitSwitchPressed()); SmartDashboard.putNumber("New State", SwitchState.newState); //These two lines are dependent of the SwitchState method SmartDashboard.putNumber("Old State", SwitchState.oldState); // get rid of them if we don't have this method - + SmartDashboard.putBoolean("Reversed", driveTrain.isReversed()); } @Override diff --git a/Steamworks/src/org/usfirst/frc/team4786/robot/RobotMap.java b/Steamworks/src/org/usfirst/frc/team4786/robot/RobotMap.java index 9549da2..65a629a 100644 --- a/Steamworks/src/org/usfirst/frc/team4786/robot/RobotMap.java +++ b/Steamworks/src/org/usfirst/frc/team4786/robot/RobotMap.java @@ -11,8 +11,7 @@ public class RobotMap { //CANTalon ports, use instead of random numbers - /*public static final int frontLeftPort = 14; - public static final int frontRightPort = 13;*/ + public static final int frontLeftPort = 13; public static final int frontRightPort = 14; @@ -29,12 +28,12 @@ public class RobotMap { public static final double MESHBOT_ROBOT_LENGTH = 2.34375; //In feet //General PID Constants - public static final int ERROR_CONSTANT_LEFT = 40; //In native units + public static final int ERROR_CONSTANT_LEFT = 20; //In native units public static final int ERROR_CONSTANT_RIGHT = 20; public static final int ALLOWABLE_TURN_ERROR = 1; //In degrees - public static final int DRIVETRAIN_ENCODER_CODES_PER_REV_LEFT = 1024; + public static final int DRIVETRAIN_ENCODER_CODES_PER_REV_LEFT = 1024; //Professor Elemental public static final int DRIVETRAIN_ENCODER_CODES_PER_REV_RIGHT = 1024; - /*public static final int DRIVETRAIN_ENCODER_CODES_PER_REV_LEFT = 360; + /*public static final int DRIVETRAIN_ENCODER_CODES_PER_REV_LEFT = 360; //Meshbot public static final int DRIVETRAIN_ENCODER_CODES_PER_REV_RIGHT = 360;*/ public static final double CLOSED_LOOP_RAMP_RATE = 0.015625; public static final int IZONE = 0; @@ -107,6 +106,7 @@ public class RobotMap { public static final int ledArduinoPort = 8; //Vision constants + public static final double finalDistanceFromPeg = .1; //for filtering //new values: R:0, G: 196. B: 120 public static final int highRedValue = 50; @@ -131,6 +131,9 @@ public class RobotMap { //for calculating angles public static final double distanceBetweenCentersOfTargets = 8.25/12; + //for PID + public static final double ALLOWABLE_CENTER_ERROR = 10; //In pixels + /* For example to map the left and right motors, you could define the following variables to use with your drivetrain subsystem. If you are using multiple modules, make sure to define both the port diff --git a/Steamworks/src/org/usfirst/frc/team4786/robot/commands/DriveToLeftGearPeg.java b/Steamworks/src/org/usfirst/frc/team4786/robot/commands/DriveToLeftGearPeg.java index af2a49b..8610ed3 100644 --- a/Steamworks/src/org/usfirst/frc/team4786/robot/commands/DriveToLeftGearPeg.java +++ b/Steamworks/src/org/usfirst/frc/team4786/robot/commands/DriveToLeftGearPeg.java @@ -14,11 +14,11 @@ public DriveToLeftGearPeg() { addSequential(new DriveToPosition(1));*/ //initial drive straight in feet - addSequential(new DriveToPosition(2)); + addSequential(new DriveToPosition(1)); //turning degrees via .22 power ratio on inside of turn for .75 seconds addSequential(new DriveArcSpeed(1, 1 * 0.22, 0.75)); //final drive straight to target - addSequential(new DriveToPosition(3)); + addSequential(new DriveToPosition(1)); // To run multiple commands at the same time, // use addParallel() diff --git a/Steamworks/src/org/usfirst/frc/team4786/robot/commands/DriveToRightGearPeg.java b/Steamworks/src/org/usfirst/frc/team4786/robot/commands/DriveToRightGearPeg.java index b944cae..1afc912 100644 --- a/Steamworks/src/org/usfirst/frc/team4786/robot/commands/DriveToRightGearPeg.java +++ b/Steamworks/src/org/usfirst/frc/team4786/robot/commands/DriveToRightGearPeg.java @@ -14,11 +14,11 @@ public DriveToRightGearPeg() { //initial drive straight in feet - addSequential(new DriveToPosition(2)); + addSequential(new DriveToPosition(1)); //turning degrees via .22 power ratio on inside of turn for .75 seconds addSequential(new DriveArcSpeed(1 * 0.22, 1 , 0.75)); //final drive straight to target - addSequential(new DriveToPosition(3)); + addSequential(new DriveToPosition(1)); // Add Commands here: // e.g. addSequential(new Command1()); diff --git a/Steamworks/src/org/usfirst/frc/team4786/robot/commands/ProcessVisionContinuously.java b/Steamworks/src/org/usfirst/frc/team4786/robot/commands/ProcessVisionContinuously.java new file mode 100644 index 0000000..34bcbe4 --- /dev/null +++ b/Steamworks/src/org/usfirst/frc/team4786/robot/commands/ProcessVisionContinuously.java @@ -0,0 +1,54 @@ +package org.usfirst.frc.team4786.robot.commands; + +import org.opencv.core.Mat; +import org.usfirst.frc.team4786.robot.Robot; +import org.usfirst.frc.team4786.robot.subsystems.MatRapper; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * + */ +public class ProcessVisionContinuously extends Command { + + public ProcessVisionContinuously() { + requires(Robot.visionImage); + } + + // Called just before this Command runs the first time + protected void initialize() { + Robot.gearPlacementCamera.setExposureManual(1); + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + + Mat frame = new Mat(); + + //Robot.outputStream.putFrame(frame); + Robot.cvSink.grabFrame(frame); + MatRapper rapperNamedMat = new MatRapper(frame); + Robot.visionImage.processImage(rapperNamedMat); + Robot.visionImage.analysis(); + SmartDashboard.putBoolean("Rectangles detected?", Robot.visionImage.getTwoTargets()); + SmartDashboard.putNumber("Right", Robot.visionImage.getDistanceRight()); + SmartDashboard.putNumber("Left", Robot.visionImage.getDistanceLeft()); + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + //it only runs in autonomous right now + return false; + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } +} diff --git a/Steamworks/src/org/usfirst/frc/team4786/robot/commands/VisionAlignWithPeg.java b/Steamworks/src/org/usfirst/frc/team4786/robot/commands/VisionAlignWithPeg.java new file mode 100644 index 0000000..1d64d9f --- /dev/null +++ b/Steamworks/src/org/usfirst/frc/team4786/robot/commands/VisionAlignWithPeg.java @@ -0,0 +1,50 @@ +package org.usfirst.frc.team4786.robot.commands; + +import org.usfirst.frc.team4786.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class VisionAlignWithPeg extends Command { + boolean check = false; + + public VisionAlignWithPeg() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.driveTrain); + } + + // Called just before this Command runs the first time + protected void initialize() { + //Robot.gearPlacementCamera.setExposureManual(1); + Robot.driveTrain.processContinuouslyInit(); + check = false; + + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + Robot.driveTrain.processContinuouslyExecute(); + check = !Robot.visionImage.getTwoTargets(); + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + //return Robot.visionImage.isCloseEnoughToPeg(); + return false; + } + + // Called once after isFinished returns true + protected void end() { + Robot.driveTrain.processContinuouslyEnd(); + System.out.println("ERROR: RUSSIAN HACKING ATTEMPT DETECTED"); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + end(); + } +} diff --git a/Steamworks/src/org/usfirst/frc/team4786/robot/subsystems/DriveTrain.java b/Steamworks/src/org/usfirst/frc/team4786/robot/subsystems/DriveTrain.java index 2c7de0d..193dece 100644 --- a/Steamworks/src/org/usfirst/frc/team4786/robot/subsystems/DriveTrain.java +++ b/Steamworks/src/org/usfirst/frc/team4786/robot/subsystems/DriveTrain.java @@ -2,6 +2,7 @@ import org.usfirst.frc.team4786.robot.Robot; import org.usfirst.frc.team4786.robot.RobotMap; import org.usfirst.frc.team4786.robot.commands.OpenLoopDrive; +import org.usfirst.frc.team4786.robot.subsystems.VisionImage.location; import com.ctre.CANTalon; import com.ctre.CANTalon.FeedbackDevice; @@ -30,6 +31,10 @@ public class DriveTrain extends Subsystem implements PIDOutput { private PIDController turnController; private double turnToAngleRate; + private PIDController visionController; + private VisionPIDSource visionPIDSource; + + //Robot will drive as if the front side is the back when reversed is true private boolean reversed; @@ -96,6 +101,15 @@ public DriveTrain(){ turnController.setOutputRange(-1.0, 1.0); turnController.setAbsoluteTolerance(RobotMap.ALLOWABLE_TURN_ERROR); turnController.setContinuous(true); + + //initialize visionController object + visionPIDSource = new VisionPIDSource(); + visionController = new PIDController(RobotMap.TurnP,RobotMap.TurnI,RobotMap.TurnD,RobotMap.TurnF, visionPIDSource, this); + visionController.setInputRange(0, 720); + visionController.setOutputRange(-0.5, 0.5); + visionController.setAbsoluteTolerance(RobotMap.ALLOWABLE_CENTER_ERROR); + visionController.setContinuous(true); + /* Add the PID Controller to the Test-mode dashboard, allowing manual * tuning of the Turn Controller's P, I and D coefficients. * Typically, only the P value needs to be modified. */ @@ -158,8 +172,8 @@ public void driveToPositionInit(double distanceToDrive){ //Make motors drive number of rotations //calculated before by convertToRotations() - frontLeft.set(-rot); - frontRight.set(rot); + frontLeft.set(rot); + frontRight.set(-rot); try { Thread.sleep(10); } catch (InterruptedException e) { @@ -168,8 +182,8 @@ public void driveToPositionInit(double distanceToDrive){ } //Make sure we inverse this right side, //otherwise, you have a spinning robot on your hands - frontLeft.set(-rot); - frontRight.set(rot); + frontLeft.set(rot); + frontRight.set(-rot); SmartDashboard.putNumber("Rotations Calculated", rot); @@ -244,7 +258,8 @@ public void driveArcSpeedEnd(){ //Some special isFinished() command stuff to not stop before the robot has even moved public boolean driveToPositionIsFinished() { - return Math.abs(frontLeft.getError()) <= RobotMap.ERROR_CONSTANT_LEFT && Math.abs(frontRight.getError()) <= RobotMap.ERROR_CONSTANT_RIGHT; + //return Math.abs(frontLeft.getError()) <= RobotMap.ERROR_CONSTANT_LEFT && Math.abs(frontRight.getError()) <= RobotMap.ERROR_CONSTANT_RIGHT; + return Math.abs(frontRight.getError()) <= RobotMap.ERROR_CONSTANT_RIGHT; } public void driveToPositionEnd(){ @@ -336,6 +351,34 @@ public void turnToAngleEnd(){ turnController.disable(); } + public void processContinuouslyInit(){ + frontLeft.changeControlMode(TalonControlMode.PercentVbus); + frontRight.changeControlMode(TalonControlMode.PercentVbus); + + visionController.enable(); + visionController.setSetpoint(RobotMap.cameraFOVWidth * .5); + } + + public void processContinuouslyExecute(){ + SmartDashboard.putBoolean("Rectangles detected?", Robot.visionImage.twoTargets); + Robot.visionImage.putValuesToSmartDashboard(); + //No idea if turnToAngleRate will actually be calculated by pidWrite() + if (Robot.visionImage.getWhereCameraIsPointing().equalsIgnoreCase("Left")) { + frontLeft.set(0.4); + frontRight.set(-0.1); + } else if (Robot.visionImage.getWhereCameraIsPointing().equalsIgnoreCase("Right")) { + frontLeft.set(-0.1); //Add 0.1 so it goes forward while aligning + frontRight.set(0.4); + } else if (Robot.visionImage.getWhereCameraIsPointing().equalsIgnoreCase("Center")) { + frontLeft.set(0.2); + frontRight.set(0.2); + } + } + + public void processContinuouslyEnd(){ + visionController.disable(); + } + //Take a distance in feet and convert to //rotations that CANTalons can take as input private double convertToRotations(double distanceInFeet){ diff --git a/Steamworks/src/org/usfirst/frc/team4786/robot/subsystems/VisionImage.java b/Steamworks/src/org/usfirst/frc/team4786/robot/subsystems/VisionImage.java index b9ccdae..25d7bbc 100644 --- a/Steamworks/src/org/usfirst/frc/team4786/robot/subsystems/VisionImage.java +++ b/Steamworks/src/org/usfirst/frc/team4786/robot/subsystems/VisionImage.java @@ -11,7 +11,7 @@ import org.usfirst.frc.team4786.robot.RobotMap; import org.usfirst.frc.team4786.robot.subsystems.MatRapper; -public class VisionImage extends Subsystem{ +public class VisionImage extends Subsystem { boolean twoTargets = false; int numOfTargets = 0; @@ -49,6 +49,14 @@ public boolean getTwoTargets(){ public double getDiffBetweenCenterXAndCamCenter(){ return diffBetweenCenterXAndCamCenter; } + + public boolean isCloseEnoughToPeg(){ + if(getDistanceLeft() < RobotMap.finalDistanceFromPeg && getDistanceRight() < RobotMap.finalDistanceFromPeg) + return true; + else + return false; + } + public double getFirstAngleToBePerpendicular(){ double d3 = .7083; double x; @@ -87,24 +95,26 @@ public location getLocationOfTarget(){//returns if targets are right, left, ahea return location.TargetsNotVisible; } } - public location getWhereCameraIsPointing(){ + public String getWhereCameraIsPointing(){ + String location = ""; if (leftRect != null && rightRect != null && centerOfCamera != 0){ if (.95*(centerOfCamera-(leftRect.x+leftRect.width)) < (rightRect.x-centerOfCamera) - && (rightRect.x-centerOfCamera) < 1.05*(centerOfCamera-(leftRect.x+leftRect.width))) - return location.Center; //camera is pointing at peg - if (rightRect.x < centerOfCamera){ - return location.Left; //camera is pointing left of peg + && (rightRect.x-centerOfCamera) < 1.05*(centerOfCamera-(leftRect.x+leftRect.width))) { + location = "Center"; //camera is pointing at peg + } else if (rightRect.x < centerOfCamera) { + location = "Left"; //camera is pointing left of peg } else if (leftRect.x > centerOfCamera) { - return location.Right; //camera is pointing right of peg + location = "Right"; //camera is pointing right of peg } else if ((centerOfCamera-(leftRect.x+leftRect.width))<(rightRect.x-centerOfCamera)) { - return location.Left; //camera is pointing left of peg + location = "Left"; //camera is pointing left of peg } else if ((rightRect.x-centerOfCamera)<(centerOfCamera-(leftRect.x+leftRect.width))) { - return location.Right; //camera is pointing right of peg + location = "Right"; //camera is pointing right of peg } else { //this should never be returned - return location.Center; + location = "Center"; } } else { - return location.TargetsNotVisible; + location = "TargetsNotVisible"; } + return location; } public void processImage(MatRapper image){ mat = image.getMat(); @@ -191,8 +201,8 @@ public void putValuesToSmartDashboard(){ //this is for testing SmartDashboard.putNumber("Right Width", rightRect.width); SmartDashboard.putNumber("Distance to left Rect", distanceToLeft); SmartDashboard.putNumber("Distance to right Rect", distanceToRight); - SmartDashboard.putNumber("Angle To Turn", Robot.visionImage.getFirstAngleToBePerpendicular()); - SmartDashboard.putNumber("First Distance", Robot.visionImage.getFirstDistanceToBePerpendicular()); + //SmartDashboard.putNumber("Angle To Turn", Robot.visionImage.getFirstAngleToBePerpendicular()); + //SmartDashboard.putNumber("First Distance", Robot.visionImage.getFirstDistanceToBePerpendicular()); } SmartDashboard.putBoolean("Rectangles detected?", twoTargets); @@ -205,4 +215,5 @@ public void putValuesToSmartDashboard(){ //this is for testing protected void initDefaultCommand() { // TODO Auto-generated method stub } + } \ No newline at end of file diff --git a/Steamworks/src/org/usfirst/frc/team4786/robot/subsystems/VisionPIDSource.java b/Steamworks/src/org/usfirst/frc/team4786/robot/subsystems/VisionPIDSource.java new file mode 100644 index 0000000..af31a7c --- /dev/null +++ b/Steamworks/src/org/usfirst/frc/team4786/robot/subsystems/VisionPIDSource.java @@ -0,0 +1,44 @@ +package org.usfirst.frc.team4786.robot.subsystems; + +import org.opencv.core.Mat; +import org.usfirst.frc.team4786.robot.Robot; +import org.usfirst.frc.team4786.robot.RobotMap; + +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSourceType; + +public class VisionPIDSource implements PIDSource{ + + PIDSourceType type; + public VisionPIDSource(){ + type = PIDSourceType.kDisplacement; + } + + @Override + public void setPIDSourceType(PIDSourceType pidSource) { + // TODO Auto-generated method stub + type = pidSource; + } + + @Override + public PIDSourceType getPIDSourceType() { + // TODO Auto-generated method stub + return type; + } + + @Override + public double pidGet() { + // TODO Auto-generated method stub + + Mat frame = new Mat(); + + //Robot.outputStream.putFrame(frame); + Robot.cvSink.grabFrame(frame); + MatRapper rapperNamedMat = new MatRapper(frame); + Robot.visionImage.processImage(rapperNamedMat); + Robot.visionImage.analysis(); + //Not certain whether this should be the absolute input or the error + return Robot.visionImage.centerX; + } + +}