This repository has been archived by the owner. It is now read-only.
Permalink
Browse files

First Commit

  • Loading branch information...
Avery-Swank committed Nov 17, 2016
1 parent 8464319 commit 76ab4cfe8c7dba52b9603914a49bbe983ec0ed48
Showing with 2,210 additions and 0 deletions.
  1. +122 −0 TylerVision.txt
  2. +49 −0 src/org/usfirst/frc/team2169/robot/OI.java
  3. +166 −0 src/org/usfirst/frc/team2169/robot/Robot.java
  4. +18 −0 src/org/usfirst/frc/team2169/robot/RobotMap.java
  5. +25 −0 src/org/usfirst/frc/team2169/robot/commands/DoNothingAutoCOM.java
  6. +48 −0 src/org/usfirst/frc/team2169/robot/commands/DriveForwardAutoCOM.java
  7. +58 −0 src/org/usfirst/frc/team2169/robot/commands/ExampleAuto.java
  8. +37 −0 src/org/usfirst/frc/team2169/robot/commands/ExampleCommand.java
  9. +19 −0 src/org/usfirst/frc/team2169/robot/commands/HighGoalAutoCOM.java
  10. +54 −0 src/org/usfirst/frc/team2169/robot/commands/ReadGoalAutoCOM.java
  11. +47 −0 src/org/usfirst/frc/team2169/robot/commands/SwitchFlywheelGo.java
  12. +58 −0 src/org/usfirst/frc/team2169/robot/commands/TeleOpAutoShoty.java
  13. +155 −0 src/org/usfirst/frc/team2169/robot/commands/TeleOpHighGoalCOM.java
  14. +101 −0 src/org/usfirst/frc/team2169/robot/commands/TurnNShoot.java
  15. +58 −0 src/org/usfirst/frc/team2169/robot/commands/climbCOM.java
  16. +34 −0 src/org/usfirst/frc/team2169/robot/commands/doNothingAuto.java
  17. +45 −0 src/org/usfirst/frc/team2169/robot/commands/driveTrainCOM.java
  18. +99 −0 src/org/usfirst/frc/team2169/robot/commands/flyWheelCOM.java
  19. +158 −0 src/org/usfirst/frc/team2169/robot/commands/highGoalAuto.java
  20. +42 −0 src/org/usfirst/frc/team2169/robot/commands/hoodCOM.java
  21. +78 −0 src/org/usfirst/frc/team2169/robot/commands/intakeCOM.java
  22. +44 −0 src/org/usfirst/frc/team2169/robot/commands/leftManipSolCOM.java
  23. +47 −0 src/org/usfirst/frc/team2169/robot/commands/liftCOM.java
  24. +47 −0 src/org/usfirst/frc/team2169/robot/commands/liftUpCOM.java
  25. +70 −0 src/org/usfirst/frc/team2169/robot/commands/pidTestCOM.java
  26. +44 −0 src/org/usfirst/frc/team2169/robot/commands/rightManipSolCOM.java
  27. +41 −0 src/org/usfirst/frc/team2169/robot/commands/velResetCOM.java
  28. +34 −0 src/org/usfirst/frc/team2169/robot/commands/velocityDownCOM.java
  29. +34 −0 src/org/usfirst/frc/team2169/robot/commands/velocityUpCOM.java
  30. +19 −0 src/org/usfirst/frc/team2169/robot/subsystems/ExampleSubsystem.java
  31. +66 −0 src/org/usfirst/frc/team2169/robot/subsystems/Server.java
  32. +69 −0 src/org/usfirst/frc/team2169/robot/subsystems/driveTrainSub.java
  33. +30 −0 src/org/usfirst/frc/team2169/robot/subsystems/endGameSub.java
  34. +62 −0 src/org/usfirst/frc/team2169/robot/subsystems/flyWheelSub.java
  35. +63 −0 src/org/usfirst/frc/team2169/robot/subsystems/imageStream.java
  36. +37 −0 src/org/usfirst/frc/team2169/robot/subsystems/intakeSub.java
  37. +32 −0 src/org/usfirst/frc/team2169/robot/subsystems/solenoidSub.java
@@ -0,0 +1,122 @@
import cv2
import numpy as np
import math
import sys
import serial
import time

cap = cv2.VideoCapture("http://10.21.69.11/mjpg/video.mjpg")
count = 0
beginDataTransfer = False
ser = serial.Serial(
port='/dev/ttyUSB0',
baudrate = 9600,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
bytesize=serial.EIGHTBITS
)
if(ser.isOpen()):
ser.close()
ser.open()
while(True):
if(ser.readline() == "begin"):
beginDataTransfer = True
if(beginDataTransfer == True):
#set frame to im
ret,im = cap.read()
#create hsv filter and apply ranges
hsv_img = cv2.cvtColor(im, cv2.COLOR_BGR2HSV)
COLOR_MIN = np.array([67, 59, 178],np.uint8)
COLOR_MAX = np.array([150, 255, 255],np.uint8)
#create threshold for hsv and contours
frame_threshed = cv2.inRange(hsv_img, COLOR_MIN, COLOR_MAX)
imgray = frame_threshed
ret,thresh = cv2.threshold(frame_threshed,127,255,0)
contours, hierarchy = cv2.findContours(thresh,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)

# Find the index of the largest contour
areas = [cv2.contourArea(c) for c in contours]
#if the array is larger than 1 object
if(len(areas)>=1):
max_index = np.argmax(areas)
cnt=contours[max_index]
M = cv2.moments(cnt)
#check that centroid exists
if(not(M['m00'] == 0)):
cx = int(M['m10']/M['m00'])
cy = int(M['m01']/M['m00'])
rect = cv2.minAreaRect(cnt)
box = cv2.cv.BoxPoints(rect)
a,b,c,d = cv2.cv.BoxPoints(rect)
#find corners and assign them
if(a[0]<cx and a[1]<cy):
topleft = a
if(b[0]<cx and b[1]<cy):
topleft = b
if(c[0]<cx and c[1]<cy):
topleft = c
if(d[0]<cx and d[1]<cy):
topleft = d

if(a[0]<cx and a[1]>cy):
bottomleft = a
if(b[0]<cx and b[1]>cy):
bottomleft = b
if(c[0]<cx and c[1]>cy):
bottomleft = c
if(d[0]<cx and d[1]>cy):
bottomleft = d
#define side lenth of bounding box
h = bottomleft[1] - topleft[1]
#math to calcuate the side length of reflective tape
sideLength = h/math.sin(90)
#get height, width and color from frame
height,width,blah = im.shape
#d = w / tan(angle)
totalDistance = (((12*height)/h)/2)/math.tan(((37.4*3.14159)/180.0)/2.0)
d = ((totalDistance*100)/12)/100
#contour stuff
box = np.int0(box)
cv2.drawContours(im,[box],0,(0,0,255),2)
#calcuate difference from centroid to edge
pixelDifference = abs(cx-width)
#pixel to inch math and width math
inchPerPixel = (((12/sideLength)*pixelDifference)/12)
widthInFeet = (((12/sideLength)*(width/2))/12)
#piecing it all together with angle math
angleToTurn = ((math.asin(widthInFeet/d))-(math.asin(inchPerPixel/d)))
angleToTurn = math.degrees(angleToTurn)
#debugging statements
if(cx>(width/2)):
turn = 'left wheels'
if(cx<(width/2)):
turn = 'right wheels'
if(cx==(width/2)):
turn = 'center'
#print 'turning', turn
#frame view for early static image tests
#cv2.imshow("Show",im)
#cv2.imshow("hsv",imgray)
#rounding angle to help buffer size
rounded = ("%.3f" % angleToTurn)
print 'angleToTurn:',angleToTurn
print ' rounded:',rounded
bin = str(rounded)

#my code
secondRounded = ("%.3f" % d)
secondBin = str(secondRounded)
finalBin = bin + '*' + secondBin

#send every tenth calc to help buffer lag
if(count >= 10):
ser.write(finalBin)
ser.write('\n')
count = 0
count += 1
#prevent program from crashing if array is less than 1 object
if(not(len(areas)>=1)):
print 'no target'
ser.write('no target\n')


@@ -0,0 +1,49 @@
package org.usfirst.frc.team2169.robot;

import org.usfirst.frc.team2169.robot.commands.SwitchFlywheelGo;
import org.usfirst.frc.team2169.robot.commands.hoodCOM;
import org.usfirst.frc.team2169.robot.commands.leftManipSolCOM;
import org.usfirst.frc.team2169.robot.commands.rightManipSolCOM;
import org.usfirst.frc.team2169.robot.commands.velocityDownCOM;
import org.usfirst.frc.team2169.robot.commands.velocityUpCOM;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.JoystickButton;

public class OI {

public Joystick leftStick;
public Joystick rightStick;
public Joystick secondaryStick;

public OI()
{
leftStick = new Joystick(0);
rightStick = new Joystick(1);
secondaryStick = new Joystick(2);

JoystickButton velocityUp = new JoystickButton(secondaryStick,11);
JoystickButton velocityDown = new JoystickButton(secondaryStick,10);
JoystickButton leftManip = new JoystickButton(secondaryStick,4);
JoystickButton rightManip = new JoystickButton(secondaryStick,5);
JoystickButton hoodManip = new JoystickButton(secondaryStick,9);
JoystickButton flywheelGo = new JoystickButton(secondaryStick,6);
//JoystickButton visionButton = new JoystickButton(secondaryStick,4);
//JoystickButton Manip = new JoystickButton(secondaryStick,5);
//JoystickButton lift = new JoystickButton(secondaryStick, 7);
//JoystickButton liftUp = new JoystickButton(secondaryStick, 8);
//JoystickButton shoot = new JoystickButton(secondaryStick, 1);

velocityUp.whenPressed(new velocityUpCOM());
velocityDown.whenPressed(new velocityDownCOM());
leftManip.whenPressed(new leftManipSolCOM());
rightManip.whenPressed(new rightManipSolCOM());
hoodManip.whenPressed(new hoodCOM());
flywheelGo.whenPressed(new SwitchFlywheelGo());
//visionButton.whenPressed(new TeleOpHighGoalCOM());
//lift.whenPressed(new liftCOM());
//liftUp.whenPressed(new liftUpCOM());
}

}

@@ -0,0 +1,166 @@
package org.usfirst.frc.team2169.robot;

import org.usfirst.frc.team2169.robot.commands.DoNothingAutoCOM;
import org.usfirst.frc.team2169.robot.commands.DriveForwardAutoCOM;
import org.usfirst.frc.team2169.robot.commands.TurnNShoot;
import org.usfirst.frc.team2169.robot.commands.climbCOM;
import org.usfirst.frc.team2169.robot.commands.driveTrainCOM;
import org.usfirst.frc.team2169.robot.commands.flyWheelCOM;
import org.usfirst.frc.team2169.robot.commands.hoodCOM;
import org.usfirst.frc.team2169.robot.commands.intakeCOM;
import org.usfirst.frc.team2169.robot.commands.leftManipSolCOM;
import org.usfirst.frc.team2169.robot.commands.rightManipSolCOM;
import org.usfirst.frc.team2169.robot.subsystems.driveTrainSub;
import org.usfirst.frc.team2169.robot.subsystems.endGameSub;
import org.usfirst.frc.team2169.robot.subsystems.flyWheelSub;
import org.usfirst.frc.team2169.robot.subsystems.intakeSub;
import org.usfirst.frc.team2169.robot.subsystems.solenoidSub;

import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.SerialPort;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.vision.AxisCamera;

public class Robot extends IterativeRobot {

//public static final ExampleSubsystem exampleSubsystem = new ExampleSubsystem();
public static final intakeSub intakeSubsystem = new intakeSub();
public static final flyWheelSub flyWheelSubsystem = new flyWheelSub();
public static final solenoidSub solenoidSubsystem = new solenoidSub();
public static final driveTrainSub driveTrainSubsystem = new driveTrainSub();
public static final endGameSub endGameSubsystem = new endGameSub();
public static OI oi;
public static int value;
public double angle;
public static double visionAngle;
public static boolean switchWasPressed;
public static boolean primaryControlled;
public static SerialPort serial;
public static Float robotAngle;
public static Float visionDistance;
public static String com;
public SendableChooser chooserAutos;
public Command intakeCom;
public Command flyWheelCom;
public Command climbCom;
public Command leftManipSolCom;
public Command rightManipSolCom;
public Command driveTrainCom;
public Command hoodCom;
public Command turnNShoot;
public Command autonomousCommand;
public Command teleOpAutoShotyCOM;
//SendableChooser autoChooser;

public static boolean flywheelRunning;

public static boolean intakesGo;
public static int standardrpmVel = 1750;

public CameraServer server;

public void robotInit() {
switchWasPressed = false;
angle = 67;
oi = new OI();

intakesGo = true;

chooserAutos = new SendableChooser();
chooserAutos.addDefault("Do Nothing", new DoNothingAutoCOM());
chooserAutos.addObject("Drive Forward", new DriveForwardAutoCOM(2.5));
//chooserAutos.addObject("High Goal", new HighGoalAutoCOM());
//chooserAutos.addObject("Pid Test", new pidTestCOM());
//chooserAutos.addObject("TurnNShoot", new TurnNShoot());
//chooserAutos.addObject("Read Goal Vision", new ReadGoalAutoCOM());
SmartDashboard.putData("Autos", chooserAutos);

value = standardrpmVel;
intakeCom = new intakeCOM();
flyWheelCom = new flyWheelCOM();
leftManipSolCom = new leftManipSolCOM();
rightManipSolCom = new rightManipSolCOM();
driveTrainCom = new driveTrainCOM();
climbCom = new climbCOM();
hoodCom = new hoodCOM();
turnNShoot = new TurnNShoot();

Robot.solenoidSubsystem.compressor.start();


//usb camera down below
//axis is already preprogrammed on smartdashboard and just needs ip address
/*server = CameraServer.getInstance();
server.setQuality(50);
server.startAutomaticCapture("cam2");*/
//serial = new SerialPort(9600, SerialPort.Port.kUSB);
//serial.enableTermination();
robotAngle = (float)0.000000;
}

public void disabledPeriodic() {
Scheduler.getInstance().run();
}

public void autonomousInit() {
Scheduler.getInstance().run();
autonomousCommand = (Command)chooserAutos.getSelected();
autonomousCommand.start();

}

public void autonomousPeriodic() {
Scheduler.getInstance().run();

}

public void teleopInit() {
if (autonomousCommand != null) autonomousCommand.cancel();

intakeCom.start();
flyWheelCom.start();
driveTrainCom.start();
//hoodCom.start();
climbCom.start();
primaryControlled = true;
/*if(!server.isAutoCaptureStarted()){
server.setQuality(50);
server.startAutomaticCapture("cam2");
}*/

//Robot.solenoidSubsystem.compressor.start();
}

public void disabledInit(){}

@SuppressWarnings("deprecation")
public void teleopPeriodic() {
//com = serial.readString();

//if(!com.isEmpty())
// System.out.println(com);

Scheduler.getInstance().run();
SmartDashboard.putBoolean("In Range", Robot.flyWheelSubsystem.flyWheelF.isEnabled());
SmartDashboard.putInt("flywheel setpoint", value);
SmartDashboard.putDouble("Rpm", Robot.flyWheelSubsystem.rpm());
SmartDashboard.putDouble("flywheel enc velocity", Robot.flyWheelSubsystem.flyWheelF.getEncVelocity());
SmartDashboard.putDouble("hang enc position", Robot.endGameSubsystem.winch.getEncPosition());
SmartDashboard.putDouble("accel X: ",Robot.driveTrainSubsystem.accel.getX());
SmartDashboard.putDouble("accel Y: ",Robot.driveTrainSubsystem.accel.getY());
SmartDashboard.putDouble("accel Z: ",Robot.driveTrainSubsystem.accel.getZ());
SmartDashboard.putBoolean("Jumped Defense", Robot.driveTrainSubsystem.jumpedDefense());
SmartDashboard.putBoolean("switch", Robot.intakeSubsystem.switchTest.get());
SmartDashboard.putBoolean("Trigger Down", Robot.oi.secondaryStick.getRawButton(1));
SmartDashboard.putBoolean("Flywheel Spinning", Robot.flywheelRunning);
}

public void testPeriodic() {
LiveWindow.run();
}
}
@@ -0,0 +1,18 @@
package org.usfirst.frc.team2169.robot;
/**
* The RobotMap is a mapping from the ports sensors and actuators are wired into
* to a variable name. This provides flexibility changing wiring, makes checking
* the wiring easier and significantly reduces the number of magic numbers
* floating around.
*/
public class RobotMap {
// For example to map the left and right motors, you could define the
// following variables to use with your drivetrain subsystem.
// public static int leftMotor = 1;
// public static int rightMotor = 2;

// If you are using multiple modules, make sure to define both the port
// number and the module. For example you with a rangefinder:
// public static int rangefinderPort = 1;
// public static int rangefinderModule = 1;
}
@@ -0,0 +1,25 @@
package org.usfirst.frc.team2169.robot.commands;

import edu.wpi.first.wpilibj.command.Command;

public class DoNothingAutoCOM extends Command {

public DoNothingAutoCOM() {
}

protected void initialize() {
}

protected void execute() {
}

protected boolean isFinished() {
return false;
}

protected void end() {
}

protected void interrupted() {
}
}
Oops, something went wrong.

0 comments on commit 76ab4cf

Please sign in to comment.