From 4555ef93f5b8c5b3ac195f86fb90a9e70f21f652 Mon Sep 17 00:00:00 2001 From: celt1125 <37297994+celt1125@users.noreply.github.com> Date: Tue, 15 Jan 2019 15:52:15 +0800 Subject: [PATCH] nothing to say I wanna play skyrim --- src/main/deploy/example.txt | 3 + src/main/java/frc/robot/Main.java | 29 ++++ src/main/java/frc/robot/Robot.java | 158 ++++++++++++++++++ src/main/java/frc/robot/RobotMap.java | 17 ++ src/main/java/frc/robot/commands/Grab.java | 27 +++ .../frc/robot/commands/MecanumDriver.java | 25 +++ .../java/frc/robot/commands/Pneumatics.java | 26 +++ 7 files changed, 285 insertions(+) create mode 100644 src/main/deploy/example.txt create mode 100644 src/main/java/frc/robot/Main.java create mode 100644 src/main/java/frc/robot/Robot.java create mode 100644 src/main/java/frc/robot/RobotMap.java create mode 100644 src/main/java/frc/robot/commands/Grab.java create mode 100644 src/main/java/frc/robot/commands/MecanumDriver.java create mode 100644 src/main/java/frc/robot/commands/Pneumatics.java diff --git a/src/main/deploy/example.txt b/src/main/deploy/example.txt new file mode 100644 index 0000000..70c79b6 --- /dev/null +++ b/src/main/deploy/example.txt @@ -0,0 +1,3 @@ +Files placed in this directory will be deployed to the RoboRIO into the +'deploy' directory in the home folder. Use the 'FileUtilities.getFilePath' wpilib function +to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java new file mode 100644 index 0000000..5b3238a --- /dev/null +++ b/src/main/java/frc/robot/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. 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 frc.robot; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. + * Unless you know what you are doing, do not modify this file except to + * change the parameter class to the startRobot call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java new file mode 100644 index 0000000..38fc351 --- /dev/null +++ b/src/main/java/frc/robot/Robot.java @@ -0,0 +1,158 @@ +package frc.robot; + +import java.util.*; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.Compressor; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.Timer; + +import org.opencv.imgproc.*; +import org.opencv.core.*; + +import edu.wpi.cscore.CvSink; +import edu.wpi.cscore.CvSource; +import edu.wpi.cscore.UsbCamera; +import edu.wpi.first.cameraserver.CameraServer; + +import frc.robot.RobotMap; +import frc.robot.commands.*; + +public class Robot extends TimedRobot { + double drivingAngle=0, rotatingAngle=0; + int distance; + + //pneumatics + boolean pneumatics_boot, pneumatics_move; + + private Joystick mstick = new Joystick(RobotMap.joystickPort); + private final Timer mtimer = new Timer(); + + // vision + Thread m_visionThread; + Mat hsv = new Mat(); + Mat Kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(10, 10)); + + boolean is_ball_find = false; + double ball_width, ball_x, ball_y; + + @Override + public void robotInit() { + m_visionThread = new Thread(() -> { + UsbCamera camera = CameraServer.getInstance().startAutomaticCapture(); + camera.setResolution(640, 480); + CvSink cvSink = CameraServer.getInstance().getVideo(); + CvSource outputStream = CameraServer.getInstance().putVideo("frame", 640, 480); + while (!Thread.interrupted()) { + Mat frame = new Mat(); + if (cvSink.grabFrame(frame) == 0) { + outputStream.notifyError(cvSink.getError()); + continue; + } + Imgproc.cvtColor(frame, hsv, Imgproc.COLOR_BGR2HSV); + Core.inRange(hsv, new Scalar(3,170,150), new Scalar(10,240,255), hsv); + Imgproc.erode(hsv, hsv, Kernel); + Imgproc.dilate(hsv, hsv, Kernel); + + List contours = new ArrayList<>(); + Imgproc.findContours(hsv, contours, new Mat(), Imgproc.RETR_TREE, Imgproc.CHAIN_APPROX_SIMPLE); + Rect rect = null; + double maxArea = 900; + + for(int i=0; i maxArea){ + rect = Imgproc.boundingRect(contours.get(i)); + maxArea = contourArea; + } + } + if(rect==null){ + is_ball_find=false; + } + else{ + is_ball_find=true; + System.out.println(is_ball_find); + System.out.println(contours.size()); + ball_width = rect.width; + ball_x = rect.x + rect.width/2; + //ball_y = rect.y + rect.height/2; + Imgproc.rectangle(frame, new Point(rect.x, rect.y), new Point(rect.x+rect.width, rect.y+rect.height), + new Scalar(255, 255, 255), 3); + distance = 33*640/rect.width; + if(ball_x<320){ + rotatingAngle = Math.toDegrees(Math.atan((ball_x-320)/distance)); + drivingAngle = -rotatingAngle-180; + } + else if(ball_x>=320){ + rotatingAngle = Math.toDegrees(Math.atan((ball_x-320)/distance)); + drivingAngle = 180-rotatingAngle; + } + } + outputStream.putFrame(frame); + } + }); + m_visionThread.setDaemon(true); + } + + @Override + public void autonomousInit() { + m_visionThread.start(); + mtimer.reset(); + mtimer.start(); + } + + @Override + public void autonomousPeriodic() { + if(is_ball_find){ + //when the direct is not precise + if(Math.abs(rotatingAngle)>5){ + MecanumDriver.drive(0, 0, rotatingAngle/360); + } + //when the direct is precise + else{ + //when the distance is too long + if(distance>5){ + MecanumDriver.drive(0.5, drivingAngle); + } + //when approach the object, grab it + else{ + Grab.in(); + } + } + } + if (mtimer.get() < 5.0) { + MecanumDriver.drive(1, 0, 0); + } + } + + @Override + public void teleopInit() { + m_visionThread.interrupt(); + pneumatics_boot=false; + pneumatics_move=false; + } + + @Override + public void teleopPeriodic() { + MecanumDriver.drive(mstick.getX(), mstick.getY(), mstick.getZ()); + if(mstick.getRawAxis(5)<-0.2){ + Grab.in(); + } + else if(mstick.getRawAxis(5)>0.2){ + Grab.out(); + } + if(mstick.getRawButton(3)){ + Pneumatics.boot(pneumatics_boot); + pneumatics_boot=!pneumatics_boot; + } + if(mstick.getRawButton(2)){ + Pneumatics.move(pneumatics_move); + pneumatics_move=!pneumatics_move; + } + } + + @Override + public void testPeriodic() { + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java new file mode 100644 index 0000000..702a5ce --- /dev/null +++ b/src/main/java/frc/robot/RobotMap.java @@ -0,0 +1,17 @@ +package frc.robot; + + +public class RobotMap{ + public static final int joystickPort = 0; + + //mecanum arcade + public static final int frontLeftPort=1; + public static final int rearLeftPort=2; + public static final int frontRightPort=3; + public static final int rearRightPort=4; + + //claw + public static final int claw1Port = 5; + public static final int claw2Port = 6; + public static final int spinPort = 7; +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/Grab.java b/src/main/java/frc/robot/commands/Grab.java new file mode 100644 index 0000000..3af2e04 --- /dev/null +++ b/src/main/java/frc/robot/commands/Grab.java @@ -0,0 +1,27 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.PWMVictorSPX; + +import frc.robot.RobotMap; + +public class Grab{ + private static SpeedController claw1 = new PWMVictorSPX(RobotMap.claw1Port); + private static SpeedController claw2 = new PWMVictorSPX(RobotMap.claw2Port); + private static PWMVictorSPX spin = new PWMVictorSPX(RobotMap.spinPort); + public void initDefaultCommand(){ + //this is for implementing the inherited abstract method + } + public static void in(){ + claw1.set(-1); + claw2.set(-1); + } + public static void out(){ + spin.set(-1); + claw1.set(1); + claw2.set(1); + } + public static void move_back(){ + spin.set(1); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/MecanumDriver.java b/src/main/java/frc/robot/commands/MecanumDriver.java new file mode 100644 index 0000000..02d48ea --- /dev/null +++ b/src/main/java/frc/robot/commands/MecanumDriver.java @@ -0,0 +1,25 @@ +package frc.robot.commands; + + +import edu.wpi.first.wpilibj.drive.MecanumDrive; +import edu.wpi.first.wpilibj.Spark; + +import frc.robot.RobotMap; + +public class MecanumDriver{ + static Spark frontLeft = new Spark(RobotMap.frontLeftPort); + static Spark rearLeft = new Spark(RobotMap.rearLeftPort); + static Spark frontRight = new Spark(RobotMap.frontRightPort); + static Spark rearRight = new Spark(RobotMap.rearRightPort); + static MecanumDrive mdrive = new MecanumDrive(frontLeft, rearLeft, frontRight, rearRight); + public static void drive(double speed, double drivingAngle){ + mdrive.drivePolar(speed, drivingAngle, 0); + } + public static void drive(double x, double y, double z){ + mdrive.driveCartesian(x, y, z); + } + + public static void stop(){ + mdrive.stopMotor(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/Pneumatics.java b/src/main/java/frc/robot/commands/Pneumatics.java new file mode 100644 index 0000000..1b3bd29 --- /dev/null +++ b/src/main/java/frc/robot/commands/Pneumatics.java @@ -0,0 +1,26 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.Compressor; +import edu.wpi.first.wpilibj.Solenoid; + +import frc.robot.RobotMap; + +public class Pneumatics{ + private static Compressor comp = new Compressor(RobotMap.compressorPort); + private static Solenoid sole = new Solenoid(RobotMap.solenoidPort); + + public void initDefaultCommand(){ + //this is for implementing the inherited abstract method + } + public static void boot(boolean stat){ + //true for on + comp.setClosedLoopControl(stat); + sole.set(stat); + } + public static void move(boolean stat){ + //true for ? + sole.set(stat); + } + +} \ No newline at end of file