diff --git a/CloneRepo.bat b/CloneRepo.bat new file mode 100644 index 0000000..8a6f229 --- /dev/null +++ b/CloneRepo.bat @@ -0,0 +1,6 @@ +del ./src/ +git init +git remote add origin https://github.com/FRC3236/DeepSpace.git +git fetch +git pull --rebase +git checkout -t origin/vision -b vision \ No newline at end of file diff --git a/src/main/java/frc/robot/CommandBase.java b/src/main/java/frc/robot/CommandBase.java index deef42e..92d3630 100644 --- a/src/main/java/frc/robot/CommandBase.java +++ b/src/main/java/frc/robot/CommandBase.java @@ -13,5 +13,7 @@ public class CommandBase { public static VisionRocket visionRocket = new VisionRocket(); public static OI controls = new OI(); public static DriveTrain drivetrain = new DriveTrain(); + public static Elevator elevator = new Elevator(); + public static Arm arm = new Arm(); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e8008d8..9706dbc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -12,9 +12,9 @@ import edu.wpi.first.wpilibj.command.Scheduler; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.commands.ExampleCommand; import frc.robot.commands.TeleopDefault; -import frc.robot.commands.TeleopTriggerControl; +import frc.robot.commands.TeleopEric; +import frc.robot.commands.TeleopVision; import frc.robot.subsystems.ExampleSubsystem; /** @@ -25,113 +25,110 @@ * project. */ public class Robot extends TimedRobot { - public static ExampleSubsystem m_subsystem = new ExampleSubsystem(); - public static CommandBase cmdBase; - public static TeleopDefault teleop; - Command m_autonomousCommand; - SendableChooser m_chooser = new SendableChooser<>(); + public enum TeleopMode { + VISION, DEFAULT, ERIC + } - /** - * This function is run when the robot is first started up and should be - * used for any initialization code. - */ - @Override - public void robotInit() { - cmdBase = new CommandBase(); - //CommandBase.drivetrain.InvertTalons(); - teleop = new TeleopDefault(); - m_chooser.setDefaultOption("Default Auto", new ExampleCommand()); - // chooser.addOption("My Auto", new MyAutoCommand()); - SmartDashboard.putData("Auto mode", m_chooser); - } + // Create a dropdown box for them all // + SendableChooser teleopChooser = new SendableChooser(); - /** - * This function is called every robot packet, no matter the mode. Use - * this for items like diagnostics that you want ran during disabled, - * autonomous, teleoperated and test. - * - *

This runs after the mode specific periodic functions, but before - * LiveWindow and SmartDashboard integrated updating. - */ - @Override - public void robotPeriodic() { - } + /** + * This function is run when the robot is first started up and should be + * used for any initialization code. + */ + @Override + public void robotInit() { + + } - /** - * This function is called once each time the robot enters Disabled mode. - * You can use it to reset any subsystem information you want to clear when - * the robot is disabled. - */ - @Override - public void disabledInit() { - } + /** + * This function is called every robot packet, no matter the mode. Use + * this for items like diagnostics that you want ran during disabled, + * autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + } - @Override - public void disabledPeriodic() { - Scheduler.getInstance().run(); - } + /** + * This function is called once each time the robot enters Disabled mode. + * You can use it to reset any subsystem information you want to clear when + * the robot is disabled. + */ + @Override + public void disabledInit() { + System.out.println("Disabled init"); + // Load all of the teleop modes into the chooser // + teleopChooser.setDefaultOption("Default Teleop", TeleopMode.DEFAULT); + teleopChooser.addOption("Vision Tracking Testing", TeleopMode.VISION); + teleopChooser.addOption("Eric's Teleop Testing", TeleopMode.ERIC); - /** - * This autonomous (along with the chooser code above) shows how to select - * between different autonomous modes using the dashboard. The sendable - * chooser code works with the Java SmartDashboard. If you prefer the - * LabVIEW Dashboard, remove all of the chooser code and uncomment the - * getString code to get the auto name from the text box below the Gyro - * - *

You can add additional auto modes by adding additional commands to the - * chooser code above (like the commented example) or additional comparisons - * to the switch structure below with additional strings & commands. - */ - @Override - public void autonomousInit() { - m_autonomousCommand = m_chooser.getSelected(); + // Add the chooser to the Shuffleboard // + SmartDashboard.putData("Teleop Mode", teleopChooser); + } - /* - * String autoSelected = SmartDashboard.getString("Auto Selector", - * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand - * = new MyAutoCommand(); break; case "Default Auto": default: - * autonomousCommand = new ExampleCommand(); break; } - */ + @Override + public void disabledPeriodic() { + Scheduler.getInstance().run(); + } - // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.start(); - } - } + /** + * This autonomous (along with the chooser code above) shows how to select + * between different autonomous modes using the dashboard. The sendable + * chooser code works with the Java SmartDashboard. If you prefer the + * LabVIEW Dashboard, remove all of the chooser code and uncomment the + * getString code to get the auto name from the text box below the Gyro + * + *

You can add additional auto modes by adding additional commands to the + * chooser code above (like the commented example) or additional comparisons + * to the switch structure below with additional strings & commands. + */ + @Override + public void autonomousInit() { + + } - /** - * This function is called periodically during autonomous. - */ - @Override - public void autonomousPeriodic() { - Scheduler.getInstance().run(); - } + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + Scheduler.getInstance().run(); + } - @Override - public void teleopInit() { - // 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 (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); - } - teleop.start(); - } + @Override + public void teleopInit() { + // Get what the teleop chooser says // + /*TeleopMode selectedMode = teleopChooser.getSelected(); + if (selectedMode == TeleopMode.DEFAULT) { + (new TeleopDefault()).start(); + } else if (selectedMode == TeleopMode.ERIC) { + (new TeleopEric()).start(); + } else if (selectedMode == TeleopMode.VISION) { + (new TeleopVision()).start(); + } + else{ + new TeleopVision().start(); + } */ + new TeleopEric().start(); + } - /** - * This function is called periodically during operator control. - */ - @Override - public void teleopPeriodic() { - Scheduler.getInstance().run(); - } + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopPeriodic() { + Scheduler.getInstance().run(); + } - /** - * This function is called periodically during test mode. - */ - @Override - public void testPeriodic() { - } + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + } } diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index e968054..402bd64 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -21,10 +21,28 @@ public class RobotMap { public static int LEFTXBOX = 0; public static int RIGHTXBOX = 1; - public static int LEFTTALONA = 0; - public static int LEFTTALONB = 1; - public static int RIGHTTALONA = 2; - public static int RIGHTTALONB = 3; + public static int LEFTVICTORA = 0; + public static int LEFTVICTORB = 1; + public static int RIGHTVICTORA = 2; + public static int RIGHTVICTORB = 3; + + + public static int ELEVATORTALON = 4; + public static int ELEVATORTALONENC = 5; + + public static int HATCHLEVELONE = 500; + public static int HATCHLEVELTWO = 1200; + public static int HATCHLEVELTHREE = 3000; + + public static int CARGOLEVELONE = 600; + public static int CARGOLEVELTWO = 1000; + public static int CARGOLEVELTHREE = 2200; + public static int CARGOLEVELSHIP = 3500; + + public static final int INTAKE = 6; + public static final int ACTUATOR = 7; + + // If you are using multiple modules, make sure to define both the port diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/ExampleCommand.java deleted file mode 100644 index 83a3fcc..0000000 --- a/src/main/java/frc/robot/commands/ExampleCommand.java +++ /dev/null @@ -1,48 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-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.commands; - -import edu.wpi.first.wpilibj.command.Command; -import frc.robot.Robot; - -/** - * An example command. You can replace me with your own command. - */ -public class ExampleCommand extends Command { - public ExampleCommand() { - // Use requires() here to declare subsystem dependencies - requires(Robot.m_subsystem); - } - - // Called just before this Command runs the first time - @Override - protected void initialize() { - } - - // Called repeatedly when this Command is scheduled to run - @Override - protected void execute() { - } - - // Make this return true when this Command no longer needs to run execute() - @Override - protected boolean isFinished() { - return false; - } - - // Called once after isFinished returns true - @Override - protected void end() { - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - @Override - protected void interrupted() { - } -} diff --git a/src/main/java/frc/robot/commands/TeleopDefault.java b/src/main/java/frc/robot/commands/TeleopDefault.java index 26dbc13..20253e0 100644 --- a/src/main/java/frc/robot/commands/TeleopDefault.java +++ b/src/main/java/frc/robot/commands/TeleopDefault.java @@ -35,7 +35,7 @@ protected void execute() { CommandBase.visionRocket.GetContourPairs(); - CommandBase.drivetrain.Drive(lateralSpeed - forwardSpeed, lateralSpeed + forwardSpeed); + CommandBase.drivetrain.drive(lateralSpeed - forwardSpeed, lateralSpeed + forwardSpeed); } // Make this return true when this Command no longer needs to run execute() diff --git a/src/main/java/frc/robot/commands/TeleopEric.java b/src/main/java/frc/robot/commands/TeleopEric.java new file mode 100644 index 0000000..4ca07ad --- /dev/null +++ b/src/main/java/frc/robot/commands/TeleopEric.java @@ -0,0 +1,263 @@ +/*----------------------------------------------------------------------------*/ +/* 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.commands; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.GenericHID.Hand; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.team3236.DriveTrainMode; +import frc.robot.CommandBase; +import frc.robot.RobotMap; +import frc.robot.subsystems.*; + + + +public class TeleopEric extends Command { + + int desiredElevatorLevelCargo = 0; + int desiredElevatorLevelHatch = 0; + + boolean canSwitchDriveMode = true; + boolean armActive = true; + + public TeleopEric() { + + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + CommandBase.elevator.zeroEncoder(); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + // Check to see if we should switch drive modes // + boolean leftStickDown = CommandBase.controls.Driver.getStickButton(Hand.kLeft); + boolean rightStickDown = CommandBase.controls.Driver.getStickButton(Hand.kRight); + if (CommandBase.controls.Driver.getBumperPressed(Hand.kLeft)) + { + CommandBase.drivetrain.switchDriveMode(); + } + + if (leftStickDown && rightStickDown) { + if (canSwitchDriveMode) { + CommandBase.drivetrain.switchDriveMode(); + canSwitchDriveMode = false; + } + } else { + // So that the drive mode isn't switching over and over again while both buttons are pressed // + canSwitchDriveMode = true; + } + + SmartDashboard.putNumber("elevator", CommandBase.elevator.getRawEncoder()); + + //double leftSpeed = CommandBase.controls.Driver.getY(Hand.kLeft); + double leftSpeed = 0; + double rightSpeed = CommandBase.controls.Driver.getY(Hand.kRight); + + CommandBase.elevator.set(rightSpeed); + + + if(CommandBase.drivetrain.getDriveMode() == DriveTrainMode.CARGO){ + switch(desiredElevatorLevelCargo){ + default: case 0: + SmartDashboard.putBoolean("Level 1", false); + SmartDashboard.putBoolean("Level 2", false); + SmartDashboard.putBoolean("Level 3", false); + SmartDashboard.putBoolean("Level 4", false); + break; + case 1: + SmartDashboard.putBoolean("Level 1", true); + SmartDashboard.putBoolean("Level 2", false); + SmartDashboard.putBoolean("Level 3", false); + SmartDashboard.putBoolean("Level 4", false); + break; + case 2: + SmartDashboard.putBoolean("Level 1", false); + SmartDashboard.putBoolean("Level 2", true); + SmartDashboard.putBoolean("Level 3", false); + SmartDashboard.putBoolean("Level 4", false); + break; + case 3: + SmartDashboard.putBoolean("Level 1", false); + SmartDashboard.putBoolean("Level 2", false); + SmartDashboard.putBoolean("Level 3", true); + SmartDashboard.putBoolean("Level 4", false); + break; + case 4: + SmartDashboard.putBoolean("Level 1", false); + SmartDashboard.putBoolean("Level 2", false); + SmartDashboard.putBoolean("Level 3", false); + SmartDashboard.putBoolean("Level 4", true); + break; + } + + if(CommandBase.controls.Driver.getYButtonPressed()) { + // Increment Elevator level here + if(desiredElevatorLevelCargo < 4) { + desiredElevatorLevelCargo++; + } + } + else if(CommandBase.controls.Driver.getBButtonPressed()) { + // Decrement Elevator level here + if (desiredElevatorLevelCargo >= 1){ + desiredElevatorLevelCargo--; + } + } + else if(CommandBase.controls.Driver.getAButton()) { + // Actually move the elevator + System.out.println(desiredElevatorLevelCargo); + + switch(desiredElevatorLevelCargo){ + default: + break; + + case 1: + CommandBase.elevator.goTo(RobotMap.CARGOLEVELONE, 0.8); + break; + + case 2: + CommandBase.elevator.goTo(RobotMap.CARGOLEVELTWO, 0.8); + break; + + case 3: + CommandBase.elevator.goTo(RobotMap.CARGOLEVELTHREE, 0.8); + break; + case 4: + CommandBase.elevator.goTo(RobotMap.CARGOLEVELSHIP, 0.8); + break; + } + } + } + else if (CommandBase.drivetrain.getDriveMode() == DriveTrainMode.HATCH) { + switch (desiredElevatorLevelHatch) { + default: + case 0: + SmartDashboard.putBoolean("Level 1", false); + SmartDashboard.putBoolean("Level 2", false); + SmartDashboard.putBoolean("Level 3", false); + + + break; + case 1: + + SmartDashboard.putBoolean("Level 1", true); + SmartDashboard.putBoolean("Level 2", false); + SmartDashboard.putBoolean("Level 3", false); + + break; + + case 2: + SmartDashboard.putBoolean("Level 1", false); + SmartDashboard.putBoolean("Level 2", true); + SmartDashboard.putBoolean("Level 3", false); + + break; + + case 3: + SmartDashboard.putBoolean("Level 1", false); + SmartDashboard.putBoolean("Level 2", false); + SmartDashboard.putBoolean("Level 3", true); + break; + case 4: + SmartDashboard.putBoolean("Level 1", false); + SmartDashboard.putBoolean("Level 2", false); + SmartDashboard.putBoolean("Level 3", false); + break; + } + + + if (CommandBase.controls.Driver.getYButtonPressed()) { + // Increment Elevator level here + if(desiredElevatorLevelHatch < 3){ + desiredElevatorLevelHatch++; + } + } + else if (CommandBase.controls.Driver.getBButtonPressed()) { + // Decrement Elevator level here + if (desiredElevatorLevelHatch >= 1){ + desiredElevatorLevelHatch--; + } + } + + + else if (CommandBase.controls.Driver.getAButton()) { + // Actually move the elevator + System.out.println(desiredElevatorLevelHatch); + + switch (desiredElevatorLevelHatch) { + default: + break; + + case 1: + CommandBase.elevator.goTo(RobotMap.HATCHLEVELONE, 0.8); + break; + + case 2: + CommandBase.elevator.goTo(RobotMap.HATCHLEVELTWO, 0.8); + break; + + case 3: + CommandBase.elevator.goTo(RobotMap.HATCHLEVELTHREE, 0.8); + break; + } + } + } + + double actuatorSpeed = CommandBase.controls.Driver.getTriggerAxis(Hand.kLeft) - CommandBase.controls.Driver.getTriggerAxis(Hand.kRight); + if (actuatorSpeed > 0.7){ + actuatorSpeed = 0.7; + } + if (actuatorSpeed < -0.7){ + actuatorSpeed = -0.7; + } + SmartDashboard.putBoolean("ACTUATOR", CommandBase.arm.getSensor(1)); + SmartDashboard.putNumber("ACTUATOR RATE", CommandBase.arm.getRate()); + CommandBase.arm.setArm(actuatorSpeed); + + if (CommandBase.controls.Driver.getBumperPressed(Hand.kRight)){ + CommandBase.elevator.set(1); + } + + if (CommandBase.arm.getActivity() == false){ + if (CommandBase.controls.Operator.getYButtonPressed()){ + CommandBase.arm.goToSensor(2); + } + if (CommandBase.controls.Operator.getBButtonPressed()){ + CommandBase.arm.goToSensor(1); + } + if (CommandBase.controls.Operator.getAButtonPressed()){ + CommandBase.arm.goToSensor(0); + } + } + else{ + return; + } + } + + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/TeleopTriggerControl.java b/src/main/java/frc/robot/commands/TeleopTriggerControl.java index 39774ad..64a337d 100644 --- a/src/main/java/frc/robot/commands/TeleopTriggerControl.java +++ b/src/main/java/frc/robot/commands/TeleopTriggerControl.java @@ -39,18 +39,19 @@ protected void execute() { if(!forward_stop){ if (forward_acceleration){ - CommandBase.drivetrain.Drive(multiplier, -multiplier); + CommandBase.drivetrain.drive(multiplier, -multiplier); + } } if(!backward_stop){ if (backward_acceleration){ - CommandBase.drivetrain.Drive(-multiplier, multiplier); + CommandBase.drivetrain.drive(-multiplier, multiplier); } } if (forward_stop || backward_stop){ - CommandBase.drivetrain.Drive(0, 0); + CommandBase.drivetrain.drive(0, 0); } //CommandBase.drivetrain.Drive(leftSpeed, rightSpeed); diff --git a/src/main/java/frc/robot/commands/TeleopVision.java b/src/main/java/frc/robot/commands/TeleopVision.java new file mode 100644 index 0000000..dbeccb8 --- /dev/null +++ b/src/main/java/frc/robot/commands/TeleopVision.java @@ -0,0 +1,58 @@ +/*----------------------------------------------------------------------------*/ +/* 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.commands; + +import java.util.ArrayList; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.subsystems.*; +import org.team3236.AssistMode; +import frc.robot.CommandBase; + +public class TeleopVision extends Command { + public TeleopVision() { + requires(CommandBase.visionRocket); + requires(CommandBase.drivetrain); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + System.out.println("Running teleop vision"); + ArrayList speeds = CommandBase.visionRocket.DriveToPair(AssistMode.CARGOSHIP, + 0.75); + System.out.println(speeds); + SmartDashboard.putNumber("Gyro", CommandBase.drivetrain.getAngle()); + CommandBase.drivetrain.drive(-speeds.get(0), speeds.get(1)); + + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java new file mode 100644 index 0000000..8ff9342 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -0,0 +1,160 @@ +/*----------------------------------------------------------------------------*/ +/* 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.subsystems; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.RobotMap; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.kauailabs.navx.frc.*; +import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.SerialPort; +import edu.wpi.first.wpilibj.SerialPort.*; +import org.team3236.DriveTrainMode; +import org.team3236.Conversion; + +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.AnalogPotentiometer; +import java.util.Arrays; +import java.beans.DesignMode; +import java.util.ArrayList; +/** + * Add your docs here. + */ + +public class Arm extends Subsystem { + private WPI_TalonSRX actuator; + private AnalogPotentiometer potentiometer; + private AnalogInput potPort = new AnalogInput(0); + private DigitalInput hallSensor0 = new DigitalInput(7); + private DigitalInput hallSensor1 = new DigitalInput(8); + private DigitalInput hallSensor2 = new DigitalInput(9); + static int previousSensor = 0; + static boolean isRunning = false; + + static private boolean ActuatorInTransit; + + public Arm() { + //potPort.setGlobalSampleRate(samplesPerSecond); + + actuator = new WPI_TalonSRX(RobotMap.ACTUATOR); + AnalogInput.setGlobalSampleRate(75); + + potPort.setAverageBits(6); + } + + public double get() { + return Math.ceil(potPort.getAverageVoltage() * 1000)/10; + } + public boolean getSensor(int sensor){ + if(sensor == 0){ + return !hallSensor0.get(); + } + else if(sensor == 1){ + return !hallSensor1.get(); + } + else { + return !hallSensor2.get(); + } + } + + public double getRate() { + return potPort.getAverageBits(); + } + + public void toggleActivity(){ + isRunning = !isRunning; + } + + public boolean getActivity(){ + return isRunning; + } + + public void setArm(double speed){ + // Update sensors + boolean sensor0Status = getSensor(0); + boolean sensor1Status = getSensor(1); + boolean sensor2Status = getSensor(2); + + if((speed < 0 && sensor2Status) || (speed > 0 && sensor0Status)){ + /* speed = 1; sensor2Status = True; + (speed < 0 && sensor2Status) = 0 & 1 = 1 = False + (speed > 0 && sensor0Status) = 1 & 0 = False + False || False = False + */ + actuator.set(speed); + } + else if (!sensor0Status && !sensor2Status){ + /* + 1 & 0 = 0 = False + */ + actuator.set(speed); + } + else{ + actuator.set(0); + } + + SmartDashboard.putBoolean("Hall Sensor 0:", sensor0Status); + SmartDashboard.putBoolean("Hall Sensor 1:", sensor1Status); + SmartDashboard.putBoolean("Hall Sensor 2:", sensor2Status); + } + + + + public void setArm(double speed, int destinationSensor){ + toggleActivity(); + while (!getSensor(destinationSensor)){ + setArm(speed); + } + previousSensor = destinationSensor; + toggleActivity(); + } + public void goToSensor(int destinationSensor){ + if(previousSensor == 2) + { + if (destinationSensor == 2){ + /* do nothing */ + } + else{ + setArm(-0.7, destinationSensor); + } + + } + + else if (previousSensor == 1){ + if (destinationSensor == 0){ + setArm(-0.7, destinationSensor); + } + else if (destinationSensor == 1){ + /* Don't move */ + } + else if (destinationSensor == 2){ + setArm(+0.7, destinationSensor); + } + + } + else if (previousSensor == 0){ + if (destinationSensor == 0){ + /* Don't move */ + } + else if (destinationSensor == 1){ + setArm(+0.7, destinationSensor); + } + else if (destinationSensor == 2){ + setArm(+0.7, destinationSensor); + } + } + + + + } + + + @Override + public void initDefaultCommand() {} +} diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 07cfbaa..76884dd 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -8,50 +8,106 @@ package frc.robot.subsystems; import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.RobotMap; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.VictorSPX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -/** - * An example subsystem. You can replace me with your own Subsystem. - */ +import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; +import com.kauailabs.navx.frc.*; +import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.SerialPort; +import edu.wpi.first.wpilibj.SerialPort.*; + +import org.team3236.DriveTrainMode; +import org.team3236.Conversion; public class DriveTrain extends Subsystem { - // Put methods for controlling this subsystem - // here. Call these from Commands. - private WPI_TalonSRX LeftTalonA = new WPI_TalonSRX(RobotMap.LEFTTALONA); - private WPI_TalonSRX LeftTalonB = new WPI_TalonSRX(RobotMap.LEFTTALONB); - private WPI_TalonSRX RightTalonA = new WPI_TalonSRX(RobotMap.RIGHTTALONA); - private WPI_TalonSRX RightTalonB = new WPI_TalonSRX(RobotMap.RIGHTTALONB); - - public void InvertTalons() - { - RightTalonA.setInverted(true); - RightTalonB.setInverted(true); - } - - public void SetLeft(double speed){ - LeftTalonA.set(speed); - LeftTalonB.set(speed); - } - - public void SetRight(double speed){ - RightTalonA.set(speed); - RightTalonB.set(speed); - } - - public void Drive(double leftSpeed, double rightSpeed){ - SetLeft(leftSpeed); - SetRight(rightSpeed); - } - - @Override - public void initDefaultCommand() { - // Set the default command for a subsystem here. - // setDefaultCommand(new MySpecialCommand()); - } - - + private WPI_VictorSPX LeftVictorA = new WPI_VictorSPX(RobotMap.LEFTVICTORA); + private WPI_VictorSPX LeftVictorB = new WPI_VictorSPX(RobotMap.LEFTVICTORB); + private WPI_VictorSPX RightVictorA = new WPI_VictorSPX(RobotMap.RIGHTVICTORA); + private WPI_VictorSPX RightVictorB = new WPI_VictorSPX(RobotMap.RIGHTVICTORB); + + private static AHRS NavX = new AHRS(SPI.Port.kMXP); + + + private DriveTrainMode driveMode = DriveTrainMode.CARGO; + + public DriveTrain() { + + // Reset the Gyro to 0 degrees // + this.resetGyro(); + + // Put the drivemode on the display // + this.updateSmartDashboard(); + } + + public void updateSmartDashboard() { + if (this.driveMode == DriveTrainMode.CARGO) { + SmartDashboard.putString("Drive Mode", "CARGO"); + } else { + SmartDashboard.putString("Drive Mode", "HATCH"); + } + } + + public double getAngle() { + return NavX.getAngle(); + } + + public double getPitch() { + return NavX.getPitch(); + } + + public void resetGyro() { + NavX.reset(); + } + + public void setDriveMode(DriveTrainMode newMode) { + this.driveMode = newMode; + this.updateSmartDashboard(); + } + + public void switchDriveMode() { + if (this.driveMode == DriveTrainMode.HATCH) { + this.driveMode = DriveTrainMode.CARGO; + } else { + this.driveMode = DriveTrainMode.HATCH; + } + this.updateSmartDashboard(); + } + + public DriveTrainMode getDriveMode() { + return this.driveMode; + } + + public void setLeft(double speed) { + if (driveMode == DriveTrainMode.HATCH) { + LeftVictorA.set(speed); + LeftVictorB.set(speed); + } else { + LeftVictorA.set(-speed); + LeftVictorB.set(-speed); + } + } + + public void setRight(double speed) { + if (driveMode == DriveTrainMode.HATCH) { + RightVictorA.set(-speed); + RightVictorB.set(-speed); + } else { + RightVictorA.set(speed); + RightVictorB.set(speed); + } + } + + public void drive(double leftSpeed, double rightSpeed){ + setLeft(leftSpeed); + setRight(rightSpeed); + + } + + @Override + public void initDefaultCommand() {} + + } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java new file mode 100644 index 0000000..fce85e1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -0,0 +1,134 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.RobotMap; +import org.team3236.Conversion; + +public class Elevator extends Subsystem { + + private WPI_TalonSRX talon, talonWithEncoder; + public Conversion conversion; + + private static final double HOLDINGCONSTANT = 0.25; // The speed at which the motors stall + // Increase/decrease the holding constant to + private static final double CAPTURERANGE = 0.7; + private static final int MAXHEIGHT = 4000; // In ticks + + public Elevator() { + talon = new WPI_TalonSRX(RobotMap.ELEVATORTALON); + talonWithEncoder = new WPI_TalonSRX(RobotMap.ELEVATORTALONENC); + + talonWithEncoder.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + + zeroEncoder(); + conversion = new Conversion(82.75, Conversion.Units.IN); // Change the 40 later // + } + + public void zeroEncoder() { + talonWithEncoder.setSelectedSensorPosition(0); + } + + public int getRawEncoder() { + return -talonWithEncoder.getSelectedSensorPosition(); + } + + public double getEncoder() { + return conversion.getInches(getRawEncoder()); + } + + public void ascend(double speed) { + double encoder = getRawEncoder(); + if (encoder / MAXHEIGHT > CAPTURERANGE && speed > 0.05) { + double captureHeight = MAXHEIGHT * CAPTURERANGE; + double motorSpeed = Math.min(gaston(MAXHEIGHT, speed, captureHeight), speed); + + talon.set(-motorSpeed); + talonWithEncoder.set(motorSpeed); + } else { + talon.set(-speed); + talonWithEncoder.set(speed); + } + } + + public void descend(double speed) { + // If the encoder is reading less than an inch from the bottom + if (getEncoder() < 15) { + talon.set(0); + talonWithEncoder.set(0); + } else { + talon.set(-speed); + talonWithEncoder.set(speed); + } + } + + // Gastons equation // + public double gaston(double destination, double rawSpeed) { + int encoder = getRawEncoder(); + destination = Math.min(destination, MAXHEIGHT); + double holdVal = HOLDINGCONSTANT; + if (encoder > destination) { + holdVal -= .1; + rawSpeed /= 4; + } + + double speed = (((destination - encoder)/destination)*Math.abs(rawSpeed)) + holdVal; + SmartDashboard.putNumber("Gaston", speed); + if (speed < 0) { + return Math.max(-1, speed); + } else { + return Math.min(1, speed); + } + } + + // Gaston's equation with custom offset + public double gaston(double destination, double rawSpeed, double offset) { + + double encoder = getRawEncoder() - offset; + destination = destination - offset; + destination = Math.min(destination, MAXHEIGHT); + + double holdVal = HOLDINGCONSTANT; + // Add more going down to increase accuracy + if (encoder > destination) { + holdVal -= .1; + rawSpeed /= 4; + } + + double speed = (((destination - encoder)/destination)*Math.abs(rawSpeed)) + holdVal; + SmartDashboard.putNumber("Gaston", speed); + if (speed < 0) { + return Math.max(-1, speed); + } else { + return Math.min(1, speed); + } + } + + public void goTo(double destination, double rawSpeed) { + // Set the position to the top if its higher than the maxheight + destination = Math.min(destination, MAXHEIGHT); + + System.out.println(destination); + double speed = gaston(destination, rawSpeed); + + SmartDashboard.putNumber("Elevator Speed", speed); + + talon.set(-speed); + talonWithEncoder.set(speed); + } + + public void set(double speed) { + if (speed >= 0) { + ascend(speed); + } else { + descend(speed); + } + } + + + + public void initDefaultCommand() {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/UltraSonic.java b/src/main/java/frc/robot/subsystems/UltraSonic.java new file mode 100644 index 0000000..a2d45c0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/UltraSonic.java @@ -0,0 +1,24 @@ +/*----------------------------------------------------------------------------*/ +/* 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.subsystems; +import edu.wpi.first.wpilibj.SerialPort; +import edu.wpi.first.wpilibj.command.Subsystem; + +/** + * Add your docs here. + */ +public class UltraSonic extends Subsystem { + // Put methods for controlling this subsystem + // here. Call these from Commands. + + @Override + public void initDefaultCommand() { + // Set the default command for a subsystem here. + // setDefaultCommand(new MySpecialCommand()); + } +} diff --git a/src/main/java/frc/robot/subsystems/VisionRocket.java b/src/main/java/frc/robot/subsystems/VisionRocket.java index d1ee9c3..83a9376 100644 --- a/src/main/java/frc/robot/subsystems/VisionRocket.java +++ b/src/main/java/frc/robot/subsystems/VisionRocket.java @@ -7,8 +7,15 @@ package frc.robot.subsystems; + import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.CommandBase; +import frc.robot.CommandBase.*; + +import org.team3236.contours.*; +import org.team3236.AssistMode; +import org.team3236.DriveTrainMode; import java.util.ArrayList; import java.util.Collections; @@ -27,8 +34,18 @@ public void initDefaultCommand() { private static NetworkTableInstance ntInst = NetworkTableInstance.getDefault(); private static NetworkTable visionTable = ntInst.getTable("contours"); + private boolean drivingAlongArc = false; + private boolean discoveredDesiredAngle = false; + private int desiredAngle = 0; + private double masterRadius = 0; + private double theta = 0; + private int normalAngle = 0; + + private static enum RocketSide { + RIGHT, LEFT + } - public ArrayList> GetContours() { + public ArrayList GetContours() { NetworkTableEntry ntwidths = visionTable.getEntry("width"); NetworkTableEntry ntheights = visionTable.getEntry("height"); NetworkTableEntry ntxs = visionTable.getEntry("x"); @@ -45,17 +62,34 @@ public ArrayList> GetContours() { Number[] ratios = ntratios.getNumberArray(def); Number[] areas = ntareas.getNumberArray(def); - ArrayList> contours = new ArrayList>(); + int smallestLen = 1000; + if (widths.length < smallestLen) { smallestLen = widths.length; } + if (heights.length < smallestLen) { smallestLen = heights.length; } + if (xs.length < smallestLen) { smallestLen = xs.length; } + if (ys.length < smallestLen) { smallestLen = ys.length; } + if (ratios.length < smallestLen) { smallestLen = ratios.length; } + if (areas.length < smallestLen) { smallestLen = areas.length; } - if (widths.length > 0) { - for (int i = 0; i < widths.length; i++) { - ArrayList newContour = new ArrayList(); + ArrayList contours = new ArrayList(); + + if (smallestLen > 0) { + for (int i = 0; i < smallestLen; i++) { + /*ArrayList newContour = new ArrayList(); newContour.add((double)xs[i]); newContour.add((double)ys[i]); newContour.add((double)widths[i]); newContour.add((double)heights[i]); newContour.add((double)areas[i]); - newContour.add((double)ratios[i]); + newContour.add((double)ratios[i]);*/ + + Contour newContour = new Contour( + (double)xs[i], + (double)ys[i], + (double)widths[i], + (double)heights[i], + (double)areas[i], + (double)ratios[i] + ); // Add this contour to the main list of contours // contours.add(newContour); @@ -64,23 +98,23 @@ public ArrayList> GetContours() { return contours; } - public ArrayList> SortContours(ArrayList> contours, int index) { - Collections.sort(contours, new Comparator>() { + public ArrayList SortContours(ArrayList contours, int index) { + Collections.sort(contours, new Comparator() { @Override - public int compare(ArrayList a1, ArrayList a2) { - return a1.get(index).compareTo(a2.get(index)); + public int compare(Contour a1, Contour a2) { + return Double.compare(a1.get(index), a2.get(index)); } }); return contours; } - public ArrayList>> GetContourPairs() { - ArrayList> contours = GetContours(); + public ArrayList GetContourPairs() { + ArrayList contours = GetContours(); // Sort the contours! // - contours = SortContours(contours, 1); + contours = SortContours(contours, 1); - ArrayList>> pairs = new ArrayList>>(); + ArrayList pairs = new ArrayList(); for (int i = 0; i < contours.size()-1; i++) { for (int j = i+1; j < contours.size(); j++) { @@ -88,18 +122,220 @@ public ArrayList>> GetContourPairs() { double secondY = contours.get(j).get(1); if ((firstY/secondY) >= 0.9) { - ArrayList> newPair = new ArrayList>(); - newPair.add(contours.get(i)); - newPair.add(contours.get(j)); - - SmartDashboard.putNumber("ContourPair1", contours.get(i).get(0)); - SmartDashboard.putNumber("ContourPair2", contours.get(j).get(0)); + ContourPair newPair = new ContourPair(contours.get(i), contours.get(j)); pairs.add(newPair); } } - } + } return pairs; } + + public ArrayList DriveToPair(AssistMode mode, double speed) { + + // speeds.get(0) is the left side of the drive train, speeds.get(1) is the right side + ArrayList speeds = new ArrayList(); + ArrayList pairs = GetContourPairs(); + + if (pairs.size() > 0) { + + if (pairs.size() > 1) { + if (mode == AssistMode.CARGOROCKET) { + + } + else if (mode == AssistMode.HATCH) { + + } + } else { + ArrayList pair = SortContours(pairs.get(0).getContours(), 0); + Contour contourA = pair.get(0); + Contour contourB = pair.get(1); + + double leftX = contourA.getX() + (double)(contourA.getWidth()/2); + double leftY = contourA.getY() + (double)(contourA.getHeight()/2); + + double rightX = contourB.getX() + (double)(contourB.getWidth()/2); + double rightY = contourB.getY() + (double)(contourB.getHeight()/2); + + NetworkTableEntry ntCamWidth = visionTable.getEntry("camWidth"); + NetworkTableEntry ntCamHeight = visionTable.getEntry("camHeight"); + double camWidth = ntCamWidth.getDouble(0); + double camHeight = ntCamHeight.getDouble(0); + + double cameraX = camWidth/2; + double pairX = leftX + (rightX-leftX)/2; + + double offset = pairX - cameraX; + + double width_X = (rightX-leftX); // Use this scale for distance + SmartDashboard.putNumber("Midpoint Distance:", width_X); + + + // Make speedScaleConstant bigger if you want robot to slow down over a longer distance, and smaller if you want it to slow down over a shorter distance + double speedScaleConstant = 9; + double speedScale = Math.min(speedScaleConstant / (contourA.getWidth() + contourB.getWidth()/2), 1.0); + double scaledSpeed = Math.min(speed * speedScale, 1.0); + + // Increase subScale to make adjustment less extreme // + double subScale = 3; + + if (Math.abs(offset) > 5) { + + if (offset > 0) { + // Angled too far to left + speeds.add(scaledSpeed); + + // Slow down right side + double scale = Math.min(1, subScale / Math.abs(offset)); + speeds.add(-(scaledSpeed * scale)); + } else { + // Angled too far to right + // Slow down left side + /*double scale = (Math.abs(offset) / (camWidth/2)); + speeds.add(scaledSpeed - scaledSpeed * scale);*/ + double scale = Math.min(1, subScale / Math.abs(offset)); + speeds.add(scaledSpeed * scale); + + speeds.add(-scaledSpeed); + + } + + } else { + speeds.add(scaledSpeed); + speeds.add(-scaledSpeed); + } + + + } + + } else { + // Set both sides to 0 + speeds.add(0.0); + speeds.add(0.0); + + } + + + if (speeds.size() != 2) { + speeds = new ArrayList(); + speeds.add(0.0); + speeds.add(0.0); + } + return speeds; + } + + public ArrayList DriveAlongArc(AssistMode mode, double speed) { + + ArrayList speeds = new ArrayList(); + + + double distanceToRocket = 72.0; // In inches + double gyroAngle = CommandBase.drivetrain.getAngle(); + int gyroAngleInt = (int)Math.round(gyroAngle); + + double width = distanceToRocket * Math.cos(gyroAngle); + double depth = distanceToRocket * Math.sin(gyroAngle); + double moddedAngle = Math.floorMod(gyroAngleInt, 360); + double moddedNormal; + + if (mode == AssistMode.CARGOROCKET) { + + } else if (mode == AssistMode.HATCH) { + + if (!discoveredDesiredAngle) { + if (moddedAngle >= 0 && moddedAngle < 90) { + desiredAngle = 60; + } else if (moddedAngle >= 90 && moddedAngle < 180) { + desiredAngle = 120; + } else if (moddedAngle >= 180 && moddedAngle < 270) { + desiredAngle = 240; + } else { + desiredAngle = 300; + } + if (moddedAngle <= desiredAngle) { + normalAngle = desiredAngle - 90; + theta = 90 - (moddedAngle-normalAngle); + } else { + normalAngle = desiredAngle + 90; + theta = 90 - (normalAngle-moddedAngle); + } + moddedNormal = Math.floorMod(normalAngle, 360); + discoveredDesiredAngle = true; + } + + + SmartDashboard.putNumber("Desired", desiredAngle); + SmartDashboard.putNumber("Modded", moddedAngle); + + // Check if our current angle is within 10 degrees of our normal angle + System.out.println("Normal: " + normalAngle); + if (!drivingAlongArc && Math.abs(moddedAngle - normalAngle) > 5) { + // Tell the drivetrain to spin // + double turnSpeed = 0.3; + if (moddedAngle < normalAngle) { + speeds.add(turnSpeed); + speeds.add(-turnSpeed); + } else { + speeds.add(-turnSpeed); + speeds.add(turnSpeed); + } + SmartDashboard.putNumber("Speed Left", speeds.get(0)); + SmartDashboard.putNumber("Speed Right", speeds.get(1)); + return speeds; + } else { + if (!drivingAlongArc) { + masterRadius = (depth/2) / Math.sin(theta); + } + drivingAlongArc = true; + // We can follow the arc now! // + double wheelBase = 23.0; + // New robot 2019 is 21.5;// + + double innerRadius = masterRadius - wheelBase; + double outerRadius = masterRadius + wheelBase; + double omega = speed/masterRadius; + + /*System.out.println("Ultrasonic " + CommandBase.drivetrain.GetDistance()); + System.out.println("Depth: " + depth); + System.out.println("Theta: " + theta); + System.out.println("Master rad: " + masterRadius); + System.out.println("Inner rad: " + innerRadius); + System.out.println("Outer rad: " + outerRadius); + System.out.println("Omega: " + omega);*/ + + double innerSpeed = -(innerRadius * omega); + double outerSpeed = -(outerRadius * omega); + + + if (Math.abs(gyroAngle - normalAngle) < 5) { + speeds.add(0.0); + speeds.add(0.0); + return speeds; + } + if (moddedAngle >= 180) { + // The inside turning circle is on the left // + speeds.add(innerSpeed); + speeds.add(outerSpeed); + } else { + // The inside turning circle is on the right // + speeds.add(outerSpeed); + speeds.add(innerSpeed); + } + + System.out.println("Speeds: I(" + innerSpeed + ") O(" + outerSpeed +")"); + SmartDashboard.putNumber("Speed Left", speeds.get(0)); + SmartDashboard.putNumber("Speed Right", speeds.get(1)); + return speeds; + } + + + } + + speeds.add(0.0); + speeds.add(0.0); + SmartDashboard.putNumber("Speed Left", speeds.get(0)); + SmartDashboard.putNumber("Speed Right", speeds.get(1)); + return speeds; + } } diff --git a/src/main/java/org/team3236/AssistMode.java b/src/main/java/org/team3236/AssistMode.java new file mode 100644 index 0000000..570f9c7 --- /dev/null +++ b/src/main/java/org/team3236/AssistMode.java @@ -0,0 +1,5 @@ +package org.team3236; + +public enum AssistMode { + HATCH, CARGOSHIP, CARGOROCKET +} \ No newline at end of file diff --git a/src/main/java/org/team3236/Conversion.java b/src/main/java/org/team3236/Conversion.java new file mode 100644 index 0000000..71d0d26 --- /dev/null +++ b/src/main/java/org/team3236/Conversion.java @@ -0,0 +1,50 @@ +package org.team3236; + +public class Conversion { + + public enum Units { + IN(1.0), FT(1/12), MM(1/25.4), CM(1/2.54), M(39.37); + + private double convVal; + + Units(double conversion) { + this.convVal = conversion; + } + + // Returns the conversion factor relative to 1 inch + public double getConversionFactor() { + return this.convVal; + } + } + + private double baseUnits; + private Units unitType; + + /** + * Creates a new conversion factor based on number of ticks per unit + * @param base the number of ticks + * @param unit a unit conversion factor + */ + public Conversion(double base, Units unit) { + baseUnits = base; + unitType = unit; + } + + /** + * Converts raw value into inches based on given units + * @param rawVal the raw value (i.e., ticks on an encoder) + * @return number of inches that is based on the units given on instantiation + */ + public double getInches(double rawVal) { + return (rawVal/this.baseUnits) * unitType.getConversionFactor(); + } + + /** + * Returns raw value based on conversion's units + * @param value the amount of units to convert + * @return number of ticks per distance unit + */ + public double getRawValue(double value) { + return value * baseUnits; + } +} \ No newline at end of file diff --git a/src/main/java/org/team3236/DriveTrainMode.java b/src/main/java/org/team3236/DriveTrainMode.java new file mode 100644 index 0000000..c97140c --- /dev/null +++ b/src/main/java/org/team3236/DriveTrainMode.java @@ -0,0 +1,5 @@ +package org.team3236; + +public enum DriveTrainMode { + CARGO, HATCH +} \ No newline at end of file diff --git a/src/main/java/org/team3236/contours/Contour.java b/src/main/java/org/team3236/contours/Contour.java new file mode 100644 index 0000000..d197d3a --- /dev/null +++ b/src/main/java/org/team3236/contours/Contour.java @@ -0,0 +1,70 @@ +package org.team3236.contours; +/** + * @author: Eric Bernard + * + */ + + +public class Contour { + private double x, y, width, height, area, ratio; + + public Contour(double x, double y, double width, double height, double area, double ratio) { + this.x = x; + this.y = y; + this.width = width; + this.height = height; + this.area = area; + this.ratio = ratio; + } + + public double get(int index) { + double valToReturn = 0; + switch(index) { + case 0: { + valToReturn = this.x; + break; + } + case 1: { + valToReturn = this.y; + break; + } + case 2: { + valToReturn = this.width; + break; + } + case 3: { + valToReturn = this.height; + break; + } + case 4: { + valToReturn = this.area; + break; + } + case 5: { + valToReturn = this.ratio; + break; + } + } + + return valToReturn; + } + + public double getX() { + return this.x; + } + public double getY() { + return this.y; + } + public double getWidth() { + return this.width; + } + public double getHeight() { + return this.height; + } + public double getArea() { + return this.area; + } + public double getRatio() { + return this.ratio; + } +} \ No newline at end of file diff --git a/src/main/java/org/team3236/contours/ContourPair.java b/src/main/java/org/team3236/contours/ContourPair.java new file mode 100644 index 0000000..db5383b --- /dev/null +++ b/src/main/java/org/team3236/contours/ContourPair.java @@ -0,0 +1,31 @@ +package org.team3236.contours; +/** + * @author: Eric Bernard + * + */ + +import java.util.ArrayList; + +import org.team3236.contours.Contour; + +public class ContourPair { + private ArrayList pair = new ArrayList(); + + public ContourPair() {} + public ContourPair(Contour... contours) { + for (int i=0; i getContours() { + return pair; + } +} \ No newline at end of file diff --git a/vendordeps/navx_frc.json b/vendordeps/navx_frc.json new file mode 100644 index 0000000..49c7d70 --- /dev/null +++ b/vendordeps/navx_frc.json @@ -0,0 +1,33 @@ +{ + "fileName": "navx_frc.json", + "name": "KauaiLabs_navX_FRC", + "version": "3.1.366", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "mavenUrls": [ + "https://repo1.maven.org/maven2/" + ], + "jsonUrl": "https://www.kauailabs.com/dist/frc/2019/navx_frc.json", + "javaDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-java", + "version": "3.1.366" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-cpp", + "version": "3.1.366", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] + } + ] +} \ No newline at end of file