diff --git a/PowerUp2018/.gitignore b/PowerUp2018/.gitignore index ae3c172..2b9a78b 100644 --- a/PowerUp2018/.gitignore +++ b/PowerUp2018/.gitignore @@ -1 +1,3 @@ /bin/ +/build/ +/dist/ \ No newline at end of file diff --git a/PowerUp2018/build/jars/WPILib.jar b/PowerUp2018/build/jars/WPILib.jar deleted file mode 100644 index 01807b6..0000000 Binary files a/PowerUp2018/build/jars/WPILib.jar and /dev/null differ diff --git a/PowerUp2018/build/jars/cscore.jar b/PowerUp2018/build/jars/cscore.jar deleted file mode 100644 index ff66874..0000000 Binary files a/PowerUp2018/build/jars/cscore.jar and /dev/null differ diff --git a/PowerUp2018/build/jars/ntcore.jar b/PowerUp2018/build/jars/ntcore.jar deleted file mode 100644 index ac47caa..0000000 Binary files a/PowerUp2018/build/jars/ntcore.jar and /dev/null differ diff --git a/PowerUp2018/build/jars/opencv.jar b/PowerUp2018/build/jars/opencv.jar deleted file mode 100644 index 5ece408..0000000 Binary files a/PowerUp2018/build/jars/opencv.jar and /dev/null differ diff --git a/PowerUp2018/build/jars/wpiutil.jar b/PowerUp2018/build/jars/wpiutil.jar deleted file mode 100644 index 6c92601..0000000 Binary files a/PowerUp2018/build/jars/wpiutil.jar and /dev/null differ diff --git a/PowerUp2018/build/org/usfirst/frc/team6351/robot/OI.class b/PowerUp2018/build/org/usfirst/frc/team6351/robot/OI.class deleted file mode 100644 index ee30e0d..0000000 Binary files a/PowerUp2018/build/org/usfirst/frc/team6351/robot/OI.class and /dev/null differ diff --git a/PowerUp2018/build/org/usfirst/frc/team6351/robot/Robot.class b/PowerUp2018/build/org/usfirst/frc/team6351/robot/Robot.class deleted file mode 100644 index 831d04e..0000000 Binary files a/PowerUp2018/build/org/usfirst/frc/team6351/robot/Robot.class and /dev/null differ diff --git a/PowerUp2018/build/org/usfirst/frc/team6351/robot/RobotMap.class b/PowerUp2018/build/org/usfirst/frc/team6351/robot/RobotMap.class deleted file mode 100644 index c9b1df9..0000000 Binary files a/PowerUp2018/build/org/usfirst/frc/team6351/robot/RobotMap.class and /dev/null differ diff --git a/PowerUp2018/build/org/usfirst/frc/team6351/robot/commands/Auto_DriveStraight.class b/PowerUp2018/build/org/usfirst/frc/team6351/robot/commands/Auto_DriveStraight.class deleted file mode 100644 index ea87dfe..0000000 Binary files a/PowerUp2018/build/org/usfirst/frc/team6351/robot/commands/Auto_DriveStraight.class and /dev/null differ diff --git a/PowerUp2018/build/org/usfirst/frc/team6351/robot/commands/FlightStickDrive.class b/PowerUp2018/build/org/usfirst/frc/team6351/robot/commands/FlightStickDrive.class deleted file mode 100644 index db94f42..0000000 Binary files a/PowerUp2018/build/org/usfirst/frc/team6351/robot/commands/FlightStickDrive.class and /dev/null differ diff --git a/PowerUp2018/build/org/usfirst/frc/team6351/robot/commands/GTADrive.class b/PowerUp2018/build/org/usfirst/frc/team6351/robot/commands/GTADrive.class deleted file mode 100644 index bcf5498..0000000 Binary files a/PowerUp2018/build/org/usfirst/frc/team6351/robot/commands/GTADrive.class and /dev/null differ diff --git a/PowerUp2018/build/org/usfirst/frc/team6351/robot/commands/GyroTurnToAngle.class b/PowerUp2018/build/org/usfirst/frc/team6351/robot/commands/GyroTurnToAngle.class deleted file mode 100644 index 7032a89..0000000 Binary files a/PowerUp2018/build/org/usfirst/frc/team6351/robot/commands/GyroTurnToAngle.class and /dev/null differ diff --git a/PowerUp2018/build/org/usfirst/frc/team6351/robot/subsystems/DriveTrain.class b/PowerUp2018/build/org/usfirst/frc/team6351/robot/subsystems/DriveTrain.class deleted file mode 100644 index 2b7ca7f..0000000 Binary files a/PowerUp2018/build/org/usfirst/frc/team6351/robot/subsystems/DriveTrain.class and /dev/null differ diff --git a/PowerUp2018/build/org/usfirst/frc/team6351/robot/subsystems/Sensors.class b/PowerUp2018/build/org/usfirst/frc/team6351/robot/subsystems/Sensors.class deleted file mode 100644 index fe0ff33..0000000 Binary files a/PowerUp2018/build/org/usfirst/frc/team6351/robot/subsystems/Sensors.class and /dev/null differ diff --git a/PowerUp2018/dist/FRCUserProgram.jar b/PowerUp2018/dist/FRCUserProgram.jar deleted file mode 100644 index a49ada4..0000000 Binary files a/PowerUp2018/dist/FRCUserProgram.jar and /dev/null differ diff --git a/PowerUp2018/src/org/usfirst/frc/team6351/robot/Robot.java b/PowerUp2018/src/org/usfirst/frc/team6351/robot/Robot.java index a327494..ef14643 100644 --- a/PowerUp2018/src/org/usfirst/frc/team6351/robot/Robot.java +++ b/PowerUp2018/src/org/usfirst/frc/team6351/robot/Robot.java @@ -10,6 +10,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.usfirst.frc.team6351.robot.commands.Auto_DriveStraight; +import org.usfirst.frc.team6351.robot.commands.Auto_DriveToCrosshairs; +import org.usfirst.frc.team6351.robot.commands.Auto_TurnToCrosshairs; import org.usfirst.frc.team6351.robot.commands.FlightStickDrive; import org.usfirst.frc.team6351.robot.commands.GTADrive; import org.usfirst.frc.team6351.robot.commands.GyroTurnToAngle; @@ -38,6 +40,7 @@ public class Robot extends TimedRobot { public static double targetX; public static double targetY; public static double targetArea; + public static double targetsVisible; public static String fms_gameData; boolean fms_dataFound = false; @@ -53,6 +56,8 @@ public void robotInit() { // chooser.addObject("My Auto", new MyAutoCommand()); m_autonomousChooser.addObject("Drive Straight", new Auto_DriveStraight(0.5,2)); m_autonomousChooser.addDefault("Turn 90 Degrees", new GyroTurnToAngle(90)); + m_autonomousChooser.addDefault("Turn To Target", new Auto_TurnToCrosshairs()); + m_autonomousChooser.addDefault("Drive To Target", new Auto_DriveToCrosshairs()); SmartDashboard.putData("Auto mode", m_autonomousChooser); // driveMode.addObject("Flight Stick Control", new FlightStickDrive()); @@ -75,6 +80,7 @@ public void disabledInit() { @Override public void disabledPeriodic() { getFMSData(); + getLimeLight(); Scheduler.getInstance().run(); } @@ -153,6 +159,7 @@ public void getLimeLight() { targetX = limelight.getEntry("tx").getDouble(0); targetY = limelight.getEntry("ty").getDouble(0); targetArea = limelight.getEntry("ta").getDouble(0); + targetsVisible = limelight.getEntry("tv").getDouble(0); } public void getFMSData() { diff --git a/PowerUp2018/src/org/usfirst/frc/team6351/robot/commands/Auto_DriveToCrosshairs.java b/PowerUp2018/src/org/usfirst/frc/team6351/robot/commands/Auto_DriveToCrosshairs.java new file mode 100644 index 0000000..ef54bad --- /dev/null +++ b/PowerUp2018/src/org/usfirst/frc/team6351/robot/commands/Auto_DriveToCrosshairs.java @@ -0,0 +1,98 @@ +package org.usfirst.frc.team6351.robot.commands; + +import org.usfirst.frc.team6351.robot.Robot; +import org.usfirst.frc.team6351.robot.RobotMap; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class Auto_DriveToCrosshairs extends Command { + + double targetX; + double targetY; + double targetArea; + double targetsVisible; + double leftMotors; + double rightMotors; + + double xChange; + double yChange; + + public Auto_DriveToCrosshairs() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.driveTrain); + } + + // Called just before this Command runs the first time + protected void initialize() { + targetX = Robot.targetX; + targetY = Robot.targetY; + targetArea = Robot.targetArea; + targetsVisible = Robot.targetsVisible; + + leftMotors = 0; + rightMotors = 0; + + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + targetX = Robot.targetX; + targetY = Robot.targetY; + targetsVisible = Robot.targetsVisible; + + double error = Math.abs(targetX); + double kP; + if (error < 3.0) { + kP = Math.pow((0.10*error), 2)+0.2; + } else { + kP = 1; + } + + if (targetsVisible == 1) { + if (targetY < -2) { + //Too Close. Go Backwards + yChange =+ RobotMap.Drive_Scaling_Auto*(-1); + } else if (targetY > 2) { + //Too Far. Go Forwards + yChange =+ RobotMap.Drive_Scaling_Auto; + } else { + } + + if (targetX < -5) { + //Turn Right + rightMotors =+ 0.2*(-1); + + + } else if (targetX > 5) { + //Turn Left + leftMotors =+ 0.2; + + } + } else { + DriverStation.reportError("Vision Auto: No Targets Found", false); + } + + Robot.driveTrain.setLeft(leftMotors); + Robot.driveTrain.setRight(rightMotors); + + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } +} diff --git a/PowerUp2018/src/org/usfirst/frc/team6351/robot/commands/Auto_TurnToCrosshairs.java b/PowerUp2018/src/org/usfirst/frc/team6351/robot/commands/Auto_TurnToCrosshairs.java new file mode 100644 index 0000000..271050e --- /dev/null +++ b/PowerUp2018/src/org/usfirst/frc/team6351/robot/commands/Auto_TurnToCrosshairs.java @@ -0,0 +1,67 @@ +package org.usfirst.frc.team6351.robot.commands; + +import org.usfirst.frc.team6351.robot.Robot; +import org.usfirst.frc.team6351.robot.RobotMap; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class Auto_TurnToCrosshairs extends Command { + + double targetX; + double targetsVisible; + + public Auto_TurnToCrosshairs() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.driveTrain); + } + + // Called just before this Command runs the first time + protected void initialize() { + targetX = Robot.targetX; + targetsVisible = Robot.targetsVisible; + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + targetX = Robot.targetX; + targetsVisible = Robot.targetsVisible; + + if (targetsVisible == 1) { + double error = Math.abs(targetX); + double kP; + if (error < 10.0) { + kP = Math.pow((0.10*error), 2)+0.2; + } else { + kP = 1; + } + if (targetX < -3){ + Robot.driveTrain.setLeft(RobotMap.Drive_Scaling_Auto*(-1)*kP); + Robot.driveTrain.setRight(RobotMap.Drive_Scaling_Auto*(-1)*kP); + } else if (targetX > 3) { + Robot.driveTrain.setLeft(RobotMap.Drive_Scaling_Auto*kP); + Robot.driveTrain.setRight(RobotMap.Drive_Scaling_Auto*kP); + } + } else { + DriverStation.reportError("Vision Auto: No Targets Found", false); + } + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } +}