Skip to content
This repository has been archived by the owner on Sep 11, 2018. It is now read-only.

Commit

Permalink
Merge branch 'release/v0.0.5'
Browse files Browse the repository at this point in the history
  • Loading branch information
@edwanvi
edwanvi committed Mar 28, 2017
2 parents 23c219f + 0f91db0 commit 220ed75
Show file tree
Hide file tree
Showing 204 changed files with 12,908 additions and 1,103 deletions.
43 changes: 37 additions & 6 deletions 3494_2017_repo/src/org/usfirst/frc/team3494/robot/AutoGenerator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,11 @@

import java.util.ArrayList;

import org.usfirst.frc.team3494.robot.commands.auto.AngleTurn;
import org.usfirst.frc.team3494.robot.commands.auto.DistanceDrive;
import org.usfirst.frc.team3494.robot.commands.auto.PIDAngleDrive;
import org.usfirst.frc.team3494.robot.commands.auto.PIDFullDrive;
import org.usfirst.frc.team3494.robot.commands.auto.XYDrive;
import org.usfirst.frc.team3494.robot.commands.gears.ToggleGearRamp;

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

Expand Down Expand Up @@ -41,16 +43,45 @@ public static ArrayList<Command> autoOne() {
*/
public static ArrayList<Command> crossBaseLine() {
ArrayList<Command> list = new ArrayList<Command>();
list.add(new DistanceDrive(100, UnitTypes.INCHES));
list.add(new DistanceDrive(-72, UnitTypes.INCHES));
return list;
}

public static ArrayList<Command> placeCenterGear() {
ArrayList<Command> list = new ArrayList<Command>();
list.add(new PIDFullDrive(110.5));
return list;
}

public static ArrayList<Command> gearPlaceAttempt() {
ArrayList<Command> list = new ArrayList<Command>();
list.add(new AngleTurn(180));
list.add(new AngleTurn(-10));
list.add(new DistanceDrive(-5, UnitTypes.INCHES));
// list.add(new XYDrive());
list.add(new PIDFullDrive(87.5));
list.add(new PIDAngleDrive(65));
list.add(new PIDFullDrive(54));
// list.add(new DistanceDrive(-60, UnitTypes.INCHES));
return list;
}

public static ArrayList<Command> gearPlaceAttemptLeft() {
ArrayList<Command> list = new ArrayList<Command>();
list.add(new PIDFullDrive(87.5));
list.add(new PIDAngleDrive(-65));
list.add(new PIDFullDrive(54));
// list.add(new DistanceDrive(-60, UnitTypes.INCHES));
return list;
}

public static ArrayList<Command> activeLeftGear() {
ArrayList<Command> list = AutoGenerator.gearPlaceAttemptLeft();
list.add(new ToggleGearRamp());
list.add(new PIDFullDrive(10));
return list;
}

public static ArrayList<Command> activeGearRight() {
ArrayList<Command> list = AutoGenerator.gearPlaceAttempt();
list.add(new ToggleGearRamp());
list.add(new PIDFullDrive(-10));
return list;
}
}
25 changes: 19 additions & 6 deletions 3494_2017_repo/src/org/usfirst/frc/team3494/robot/OI.java
View file
Original file line number Diff line number Diff line change
@@ -1,14 +1,16 @@
package org.usfirst.frc.team3494.robot;

import org.usfirst.frc.team3494.robot.commands.auto.DistanceDrive;
import org.usfirst.frc.team3494.robot.commands.climb.Climb;
import org.usfirst.frc.team3494.robot.commands.climb.ClimberToggle;
import org.usfirst.frc.team3494.robot.commands.climb.StopClimber;
import org.usfirst.frc.team3494.robot.commands.drive.HoldDriveTrain;
import org.usfirst.frc.team3494.robot.commands.gears.HoldInState;
import org.usfirst.frc.team3494.robot.commands.gears.HoldInState_Forward;
import org.usfirst.frc.team3494.robot.commands.gears.SetReverse;
import org.usfirst.frc.team3494.robot.commands.gears.ToggleGearRamp;
import org.usfirst.frc.team3494.robot.commands.intake.SwitchPosition;
import org.usfirst.frc.team3494.robot.commands.turret.Shoot;

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

Expand All @@ -26,6 +28,8 @@ public class OI {
// Button button = new JoystickButton(stick, buttonNumber);
public final XboxController xbox = new XboxController(RobotMap.XBOX_ONE);
public final XboxController xbox_2 = new XboxController(RobotMap.XBOX_TWO);
public final Joystick stick_l = new Joystick(3);
public final Joystick stick_r = new Joystick(4);
// There are a few additional built in buttons you can use. Additionally,
// by subclassing Button you can create custom triggers and bind those to
// commands the same as any other Button.
Expand All @@ -46,6 +50,7 @@ public class OI {
// until it is finished as determined by it's isFinished method.
// button.whenReleased(new ExampleCommand());
public JoystickButton xbox_a = new JoystickButton(xbox, 1);
public JoystickButton xbox_a_2 = new JoystickButton(xbox_2, 1);

public JoystickButton xbox_lt = new JoystickButton(xbox, 5);

Expand All @@ -59,20 +64,28 @@ public class OI {
public JoystickButton xbox_y_2 = new JoystickButton(xbox_2, 4);

public JoystickButton xbox_x = new JoystickButton(xbox, 3);
public JoystickButton xbox_x_2 = new JoystickButton(xbox_2, 3);

public JoystickButton xbox_select_2 = new JoystickButton(xbox_2, 7);
public JoystickButton xbox_start_2 = new JoystickButton(xbox_2, 8);

public OI() {
// Ready Player One
xbox_b.whileHeld(new Climb(DriveDirections.UP));
xbox_b.whenReleased(new StopClimber());
xbox_x.whileHeld(new HoldInState());
xbox_a.whenPressed(new DistanceDrive(-12, UnitTypes.INCHES));
// Ready Player Two
xbox_b_2.whenPressed(new SwitchPosition());
// Climb controls
xbox_a_2.whileActive(new Climb(DriveDirections.UP));
xbox_a_2.whenReleased(new StopClimber());

xbox_b_2.whenPressed(new SetReverse());

xbox_x_2.whileHeld(new HoldInState_Forward());

xbox_y_2.whenPressed(new ToggleGearRamp());

xbox_rt_2.whenPressed(new Shoot());
xbox_rt_2.whenReleased(new Shoot(0));

xbox_select_2.whenPressed(new ClimberToggle());
xbox_start_2.whileHeld(new HoldDriveTrain());
}
Expand Down
155 changes: 144 additions & 11 deletions 3494_2017_repo/src/org/usfirst/frc/team3494/robot/Robot.java
View file
Original file line number Diff line number Diff line change
@@ -1,17 +1,28 @@
package org.usfirst.frc.team3494.robot;

import java.util.ArrayList;

import org.opencv.core.MatOfPoint;
import org.opencv.core.Point;
import org.opencv.core.Rect;
import org.opencv.imgproc.Imgproc;
import org.usfirst.frc.team3494.robot.commands.auto.ConstructedAuto;
import org.usfirst.frc.team3494.robot.commands.auto.NullAuto;
import org.usfirst.frc.team3494.robot.commands.auto.PIDAngleDrive;
import org.usfirst.frc.team3494.robot.commands.auto.PIDFullDrive;
import org.usfirst.frc.team3494.robot.subsystems.Climber;
import org.usfirst.frc.team3494.robot.subsystems.Drivetrain;
import org.usfirst.frc.team3494.robot.subsystems.GearTake;
import org.usfirst.frc.team3494.robot.subsystems.Intake;
import org.usfirst.frc.team3494.robot.subsystems.GearTake_2;
import org.usfirst.frc.team3494.robot.subsystems.Kompressor;
import org.usfirst.frc.team3494.robot.subsystems.Turret;
import org.usfirst.frc.team3494.robot.vision.GripPipeline;

import com.ctre.CANTalon;
import com.kauailabs.navx.frc.AHRS;

import edu.wpi.cscore.UsbCamera;
import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.PowerDistributionPanel;
import edu.wpi.first.wpilibj.Preferences;
Expand All @@ -21,6 +32,7 @@
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.VisionThread;

/**
* The VM is configured to automatically run this class, and to call the
Expand All @@ -35,9 +47,8 @@ public class Robot extends IterativeRobot {
public static Drivetrain driveTrain;
public static Climber climber;
public static Turret turret;
public static Intake intake;
public static Kompressor kompressor;
public static GearTake gearTake;
public static GearTake_2 gearTake;
/**
* The gyro board mounted to the RoboRIO.
*
Expand All @@ -50,30 +61,93 @@ public class Robot extends IterativeRobot {
public static SendableChooser<Command> chooser;
public static Preferences prefs;

public static UsbCamera camera_0;
public static UsbCamera camera_1;
// Vision items
private static final int IMG_WIDTH = 320;
@SuppressWarnings("unused")
private static final int IMG_HEIGHT = 240;
VisionThread visionThread;
public double centerX = 0.0;
public double absolutelyAverage = 0.0;
@SuppressWarnings("unused")
private ArrayList<MatOfPoint> filteredContours;
private ArrayList<Double> averages = new ArrayList<Double>();

private final Object imgLock = new Object();

/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
*/
@Override
public void robotInit() {
System.out.println("Hello FTAs, how are you doing?");
System.out.println("Because I'm a QUADRANGLE.");
chooser = new SendableChooser<Command>();
driveTrain = new Drivetrain();
climber = new Climber();
climber.disengagePTO();
turret = new Turret();
kompressor = new Kompressor();
intake = new Intake();
gearTake = new GearTake();
gearTake = new GearTake_2();
oi = new OI();
ahrs = new AHRS(SerialPort.Port.kMXP);
Robot.climber.disengagePTO();
// Auto programs come after all subsystems are created
chooser.addDefault("To the baseline!", new ConstructedAuto(AutoGenerator.crossBaseLine()));
chooser.addObject("Other command", new ConstructedAuto(AutoGenerator.crossBaseLine()));
@SuppressWarnings("unused")
UsbCamera camera_0 = CameraServer.getInstance().startAutomaticCapture(0);
chooser.addObject("Center Gear Placer", new ConstructedAuto(AutoGenerator.placeCenterGear()));
chooser.addObject("[beta] Right Gear Attempt", new ConstructedAuto(AutoGenerator.gearPlaceAttempt()));
chooser.addObject("[beta] Left Gear Attempt", new ConstructedAuto(AutoGenerator.gearPlaceAttemptLeft()));
chooser.addObject("Follow the shiny", null);
chooser.addObject("Do nothing", new NullAuto());
chooser.addObject("PID Test - turn 90 degrees", new PIDAngleDrive(90));
chooser.addObject("PID Test - drive straight", new PIDFullDrive(36));
chooser.addObject("Active Gear placer - Robot turn left", new ConstructedAuto(AutoGenerator.activeLeftGear()));
chooser.addObject("Active Gear placer - Robot turn right", new ConstructedAuto(AutoGenerator.activeGearRight()));
// put chooser on DS
SmartDashboard.putData("Auto mode", chooser);
SmartDashboard.putData("AUTO CHOOSER", chooser);
// get preferences
prefs = Preferences.getInstance();
camera_0 = CameraServer.getInstance().startAutomaticCapture("Gear View", 0);
camera_0.setExposureManual(20);
// camera_1 = CameraServer.getInstance().startAutomaticCapture("Intake
// View", 1);
// Create and start vision thread
visionThread = new VisionThread(camera_0, new GripPipeline(), pipeline -> {
if (!pipeline.filterContoursOutput().isEmpty()) {
if (pipeline.filterContoursOutput().size() >= 2) {
MatOfPoint firstCont = pipeline.filterContoursOutput().get(0);
MatOfPoint secondCont = pipeline.filterContoursOutput().get(1);
double average_y_one = 0;
for (Point p : firstCont.toList()) {
average_y_one += p.y;
}
double average_y_two = 0;
for (Point p : secondCont.toList()) {
average_y_two += p.y;
}
// divide by number of points to give actual average
average_y_two = average_y_two / secondCont.toList().size();
average_y_one = average_y_one / firstCont.toList().size();
Rect r = Imgproc.boundingRect(pipeline.findContoursOutput().get(0));
synchronized (imgLock) {
centerX = r.x + (r.width / 2);
filteredContours = pipeline.filterContoursOutput();
// add averages to list
averages.add(average_y_one);
averages.add(average_y_two);
}
} else {
Rect r = Imgproc.boundingRect(pipeline.findContoursOutput().get(0));
synchronized (imgLock) {
centerX = r.x + (r.width / 2);
filteredContours = pipeline.filterContoursOutput();
}
}
}
});
visionThread.start();
}

/**
Expand Down Expand Up @@ -107,7 +181,19 @@ public void autonomousInit() {
autonomousCommand = chooser.getSelected();
// schedule the autonomous command (example)
if (autonomousCommand != null) {
System.out.println("Selected command " + chooser.getSelected().getName());
autonomousCommand.start();
} else {
System.out.println("Defaulting to track the shiny");
}
// set ramps
for (CANTalon t : Robot.driveTrain.leftSide) {
t.setVoltageRampRate(0);
t.enableBrakeMode(true);
}
for (CANTalon t : Robot.driveTrain.rightSide) {
t.setVoltageRampRate(0);
t.enableBrakeMode(true);
}
}

Expand All @@ -116,18 +202,59 @@ public void autonomousInit() {
*/
@Override
public void autonomousPeriodic() {
Scheduler.getInstance().run();
SmartDashboard.putData(Scheduler.getInstance());
if (autonomousCommand != null) {
Scheduler.getInstance().run();
} else {
double centerX;
synchronized (imgLock) {
centerX = this.centerX;
System.out.println("CenterX: " + this.centerX);
}
double turn = centerX - (Robot.IMG_WIDTH / 2);
// drive with turn
System.out.println("Turn: " + turn);
Robot.driveTrain.wpiDrive.arcadeDrive(0.5, (turn * 0.005) * -1);
}
SmartDashboard.putNumber("[left] distance", Robot.driveTrain.getLeftDistance(UnitTypes.RAWCOUNT));
SmartDashboard.putNumber("[left] distance inches", Robot.driveTrain.getLeftDistance(UnitTypes.INCHES));

SmartDashboard.putNumber("[right] distance", Robot.driveTrain.getRightDistance(UnitTypes.RAWCOUNT));
SmartDashboard.putNumber("[right] distance inches", Robot.driveTrain.getRightDistance(UnitTypes.INCHES));

SmartDashboard.putNumber("Motor 0", Robot.pdp.getCurrent(0));
SmartDashboard.putNumber("Motor 1", Robot.pdp.getCurrent(1));
SmartDashboard.putNumber("Motor 2", Robot.pdp.getCurrent(2));

SmartDashboard.putNumber("Talon Distance Right", Robot.driveTrain.rightSide[0].getPosition());
SmartDashboard.putNumber("Talon Distance Left", Robot.driveTrain.leftSide[0].getPosition());

SmartDashboard.putNumber("Motor 13", Robot.pdp.getCurrent(13));
SmartDashboard.putNumber("Motor 14", Robot.pdp.getCurrent(14));
SmartDashboard.putNumber("Motor 15", Robot.pdp.getCurrent(15));
}

@Override
public void teleopInit() {
Robot.gearTake.setGrasp(Value.kOff);
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (autonomousCommand != null) {
autonomousCommand.cancel();
}
camera_0.setExposureAuto();
camera_0.setWhiteBalanceAuto();
// set ramps
for (CANTalon t : Robot.driveTrain.leftSide) {
t.setVoltageRampRate(Drivetrain.RAMP);
t.enableBrakeMode(true);
}
for (CANTalon t : Robot.driveTrain.rightSide) {
t.setVoltageRampRate(Drivetrain.RAMP);
t.enableBrakeMode(true);
}
}

/**
Expand All @@ -146,13 +273,19 @@ public void teleopPeriodic() {
SmartDashboard.putNumber("[right] distance", Robot.driveTrain.getRightDistance(UnitTypes.RAWCOUNT));
SmartDashboard.putNumber("[right] distance inches", Robot.driveTrain.getRightDistance(UnitTypes.INCHES));

SmartDashboard.putNumber("Talon Distance Right", Robot.driveTrain.rightSide[0].getPosition());
SmartDashboard.putNumber("Talon Distance Left", Robot.driveTrain.leftSide[0].getPosition());

SmartDashboard.putNumber("Motor 0", Robot.pdp.getCurrent(0));
SmartDashboard.putNumber("Motor 1", Robot.pdp.getCurrent(1));
SmartDashboard.putNumber("Motor 2", Robot.pdp.getCurrent(2));

SmartDashboard.putNumber("Motor 13", Robot.pdp.getCurrent(13));
SmartDashboard.putNumber("Motor 14", Robot.pdp.getCurrent(14));
SmartDashboard.putNumber("Motor 15", Robot.pdp.getCurrent(15));

SmartDashboard.putNumber("Climber Motor", Robot.pdp.getCurrent(RobotMap.CLIMBER_MOTOR_PDP));
SmartDashboard.putBoolean("line break", Robot.gearTake.lb.getBroken());
}

/**
Expand Down
Loading

0 comments on commit 220ed75

Please sign in to comment.