Skip to content

nothing to say #1

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
hs1ang-hsu wants to merge 1 commit into
base: master
Choose a base branch
from
Open

nothing to say #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions src/main/deploy/example.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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.
29 changes: 29 additions & 0 deletions src/main/java/frc/robot/Main.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}
158 changes: 158 additions & 0 deletions src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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<MatOfPoint> 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<contours.size(); i++){
Mat cnt = contours.get(i);
double contourArea = Imgproc.contourArea(cnt);
if(contourArea > 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() {
}
}
17 changes: 17 additions & 0 deletions src/main/java/frc/robot/RobotMap.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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;
}
27 changes: 27 additions & 0 deletions src/main/java/frc/robot/commands/Grab.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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);
}
}
25 changes: 25 additions & 0 deletions src/main/java/frc/robot/commands/MecanumDriver.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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();
}
}
26 changes: 26 additions & 0 deletions src/main/java/frc/robot/commands/Pneumatics.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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);
}

}