diff --git a/.idea/jarRepositories.xml b/.idea/jarRepositories.xml
index fecad9f..8f95b84 100644
--- a/.idea/jarRepositories.xml
+++ b/.idea/jarRepositories.xml
@@ -71,5 +71,10 @@
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
index 1c80b4e..b808dca 100644
--- a/.idea/misc.xml
+++ b/.idea/misc.xml
@@ -4,5 +4,5 @@
-
+
\ No newline at end of file
diff --git a/.idea/modules/robot-2022.iml b/.idea/modules/robot-2022.iml
new file mode 100644
index 0000000..248b003
--- /dev/null
+++ b/.idea/modules/robot-2022.iml
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/build.gradle b/build.gradle
index aa77ea2..23c46ee 100644
--- a/build.gradle
+++ b/build.gradle
@@ -2,15 +2,14 @@ import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO
plugins {
id "java"
- id "edu.wpi.first.GradleRIO" version "2022.1.1"
+ id "edu.wpi.first.GradleRIO" version "2022.4.1"
}
sourceCompatibility = JavaVersion.VERSION_11
targetCompatibility = JavaVersion.VERSION_11
def ROBOT_MAIN_CLASS = "frc.robot.Main"
-
-// Define my targets (RoboRIO) and artifacts (deployable files)
+/// Define my targets (RoboRIO) and artifacts (deployable files)
// This is added by GradleRIO's backing project DeployUtils.
deploy {
targets {
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index f016cfc..336d644 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -5,79 +5,147 @@
package frc.robot;
import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.autonomous.*;
import frc.robot.autonomous.GenericAutonomous;
+import frc.robot.command.*;
import frc.robot.generic.GenericRobot;
import frc.robot.generic.Lightning;
-import frc.robot.generic.TurretBot;
+
+import java.util.Arrays;
+import java.util.Map;
+import java.util.function.Function;
+import java.util.stream.Collectors;
public class Robot extends TimedRobot {
//Instantiate autonomous once, don't create unnecessary duplicates
//Add new Autos here when they're authored
public static final GenericAutonomous
- autoArc = new autoArc(),
- simpleATerminal = new SimpleATerminal(),
- simpleBTerminal = new SimpleBTerminal(),
- simpleCTerminal = new SimpleCTerminal();
-
- GenericRobot robot = new TurretBot();
+ autoArc = new autoArc(),
+ ATerminalReturn = new BallAtoTerminalReturn(),
+ simpleBTerminal = new BallBtoTerminal(),
+ simpleCTerminal = new BallCtoTerminal(),
+ CTerminalReturn = new BallCtoTerminalReturn(),
+ BTerminalReturn = new BallBtoTerminalReturn(),
+ simpleB = new BallSimpleB(),
+ calibration = new Calibration(),
+ shortRun = new ShortRun();
+
+ GenericRobot robot = new Lightning();
Joystick joystick = new Joystick(0);
+ GenericCommand command = new Hang();
Joystick xbox = new Joystick(1);
- GenericAutonomous autonomous = autoArc;
+ GenericAutonomous autonomous = CTerminalReturn;
+ GenericCommand testHang = new HangWithoutAlign();
int averageTurretXSize = 2;
- double[] averageX = new double [averageTurretXSize];
+ double[] averageX = new double[averageTurretXSize];
+
- double turretx;
- double turrety;
- double turretarea;
- double turretv;
int counter = 0;
double average;
- double currentTurretPower;
+ double turretPower;
+ boolean hang = false;
+ int countHang = 0;
+ int countShoot = 0;
+ boolean shoot = false;
+ boolean reset = true;
+ double maxCurrentLeftA = 0;
+ double maxCurrentLeftB = 0;
+ double maxCurrentRightA = 0;
+ double maxCurrentRightB = 0;
+ boolean ActuallyHanging = false;
+
+ double driveLeft = 0;
+ double driveRight = 0;
+ double joystickX;
+ double joystickY;
+ double cutoff = 0.05;
+ double scaleFactor = 1.0;
+
+ int leftAxis = 1;
+ int rightAxis = 5;
+ double tolerance = 0.8;
+ double defaultClimbPower = 0.2;
+
+ double shooterTargetRPM = 0;
+
+ double defCollectorPower = 1;
+ double defIndexerPower = 1;
+ double curCollector;
+ double curIndexer;
+
+ double targetRPM = 0;
+ double turretPitch = 0;
+
+ boolean turnTo45 = false;
+ boolean turnTo225 = false;
+
+ boolean isShooting = false;
PIDController turretPIDController;
+ @Override
+ public void robotInit() {
+ System.out.println("Klaatu barada nikto");
+ robot.setTurretPitchPosition(0);
+ }
- @Override public void robotInit() {}
+ @Override
+ public void robotPeriodic() {
- @Override public void robotPeriodic() {
+ if (countShoot == 0){
+ isShooting = false;
+ }
+ else{
+ isShooting = robot.isReadyToShoot();
+ }
- NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
- NetworkTableEntry tx = table.getEntry("tx");
- NetworkTableEntry ty = table.getEntry("ty");
- NetworkTableEntry ta = table.getEntry("ta");
- NetworkTableEntry tv = table.getEntry("tv");
+ if (robot.getLeftACurrent() > maxCurrentLeftA) {
+ maxCurrentLeftA = robot.getLeftACurrent();
+ }
+ if (robot.getLeftBCurrent() > maxCurrentLeftB) {
+ maxCurrentLeftB = robot.getLeftBCurrent();
+ }
+ if (robot.getRightACurrent() > maxCurrentRightA) {
+ maxCurrentRightA = robot.getRightACurrent();
+ }
+ if (robot.getRightBCurrent() > maxCurrentRightB) {
+ maxCurrentRightB = robot.getRightBCurrent();
+ }
-//read values periodically
- turretx = tx.getDouble(0.0);
- turrety = ty.getDouble(0.0);
- turretarea = ta.getDouble(0.0);
- turretv = tv.getDouble(0.0);
- SmartDashboard.putNumber("tv", turretv);
+ SmartDashboard.putNumber("XBOX AXIS DEBUG - 0 ", xbox.getRawAxis(0));
+ SmartDashboard.putNumber("XBOX AXIS DEBUG - 1 ", xbox.getRawAxis(1));
+ SmartDashboard.putNumber("XBOX AXIS DEBUG - 2 ", xbox.getRawAxis(2));
+ SmartDashboard.putNumber("XBOX AXIS DEBUG - 3 ", xbox.getRawAxis(3));
- SmartDashboard.putNumber("LimelightX", turretx);
- SmartDashboard.putNumber("LimelightY", turrety);
- SmartDashboard.putNumber("LimelightArea", turretarea);
+
+ SmartDashboard.putNumber("LeftACurrentMax", maxCurrentLeftA);
+ SmartDashboard.putNumber("LeftBCurrentMax", maxCurrentLeftB);
+ SmartDashboard.putNumber("RightACurrentMax", maxCurrentRightA);
+ SmartDashboard.putNumber("RightBCurrentMax", maxCurrentRightB);
+
+ //SmartDashboard.putNumber("LimelightX", robot.getTargetX());
+ //SmartDashboard.putNumber("LimelightY", robot.getTargetY());
+ SmartDashboard.putNumber("LimelightArea", robot.getTargetArea());
SmartDashboard.putNumber("Drive left pct", robot.getDriveLeftPercentage());
SmartDashboard.putNumber("Drive right pct", robot.getDriveRightPercentage());
SmartDashboard.putNumber("Drive left rpm", robot.getDriveLeftRPM());
SmartDashboard.putNumber("Drive right rpm", robot.getDriveRightRPM());
- SmartDashboard.putNumber("Left encoder Ticks", robot.encoderTicksLeftDrive());
- SmartDashboard.putNumber("Right encoder Ticks", robot.encoderTicksRightDrive());
+ SmartDashboard.putNumber("Left encoder Ticks A", robot.encoderTicksLeftDriveA());
+ SmartDashboard.putNumber("Right encoder Ticks A", robot.encoderTicksRightDriveA());
+
+ SmartDashboard.putNumber("Left encoder Ticks B", robot.encoderTicksLeftDriveB());
+ SmartDashboard.putNumber("Right encoder Ticks B", robot.encoderTicksRightDriveB());
SmartDashboard.putNumber("Left encoder Inches", robot.getDriveDistanceInchesLeft());
SmartDashboard.putNumber("Right encoder Inches", robot.getDriveDistanceInchesRight());
@@ -103,20 +171,22 @@ public class Robot extends TimedRobot {
//SmartDashboard.getBoolean("Sees target?", robot.isTargetFound());
+ SmartDashboard.putBoolean("Vision target found", robot.isTargetFound());
SmartDashboard.putNumber("Vision target x", robot.getTargetX());
+ SmartDashboard.putNumber("Vision target Average", average);
SmartDashboard.putNumber("Vision target y", robot.getTargetY());
SmartDashboard.putNumber("Vision target angle", robot.getTargetAngle());
SmartDashboard.putNumber("Vision target dist", robot.getTargetDistance());
- SmartDashboard.putNumber("Turret direction angle ticks", robot.getTurretAngle());
- SmartDashboard.putNumber("Turret direction angle degrees", robot.getTurretAngleDegrees());
- SmartDashboard.putNumber("Alternate turret angle ticks", robot.getAlternateTurretAngle());
+ //SmartDashboard.putNumber("Turret direction angle ticks", robot.getTurretAngle());
+ //SmartDashboard.putNumber("Turret direction angle degrees", robot.getTurretAngleDegrees());
+ SmartDashboard.putNumber("Alternate turret angle degrees", robot.getAlternateTurretAngle());
SmartDashboard.putNumber("Turret direction motor pct", robot.getTurretPowerPct());
- SmartDashboard.putNumber("Turret direction motor pct", robot.getTurretPowerPct());
+ //SmartDashboard.putNumber("Turret direction motor pct", robot.getTurretPowerPct());
- SmartDashboard.putNumber("Turret pitch angle", robot.getTurretPitchAngle());
+ SmartDashboard.putNumber("Turret pitch position", robot.getTurretPitchPosition());
SmartDashboard.putNumber("Turret pitch motor pct", robot.getTurretPitchPowerPct());
SmartDashboard.putNumber("Shooter top motor pct", robot.getShooterPowerPctTop());
@@ -140,236 +210,526 @@ public class Robot extends TimedRobot {
SmartDashboard.putNumber("Joystick raw Y", joystick.getY());
SmartDashboard.putNumber("Autonomous Step", autonomous.autonomousStep);
+ SmartDashboard.putString("Autonomous Program", autonomous.getClass().getName());
+
+ SmartDashboard.putNumber("leftEncoderRaw", robot.encoderTicksLeftDriveA());
+ SmartDashboard.putNumber("rightEncoderRaw", robot.encoderTicksRightDriveA());
+ SmartDashboard.putBoolean("leftTapeSensor", robot.getFloorSensorLeft());
+ SmartDashboard.putBoolean("rightTapeSensor", robot.getFloorSensorRight());
+ SmartDashboard.putBoolean("leftCLimberSensor", robot.getClimbSensorLeft());
+ SmartDashboard.putBoolean("rightClimberSensor", robot.getClimbSensorRight());
+
+ SmartDashboard.putNumber("rightArmEncoder", robot.armHeightRight());
+ SmartDashboard.putNumber("leftArmEncoder", robot.armHeightLeft());
+
+ SmartDashboard.putBoolean("hang", hang);
+ SmartDashboard.putNumber("count", countHang);
+
+ SmartDashboard.putNumber("CommandStep", command.commandStep);
+
+ SmartDashboard.putBoolean("Robot is Shooting?", isShooting);
+
}
- @Override public void autonomousInit() {
+ @Override
+ public void autonomousInit() {
autonomous.autonomousInit(robot);
}
- @Override public void autonomousPeriodic() {
+ @Override
+ public void autonomousPeriodic() {
autonomous.autonomousPeriodic(robot);
}
- @Override public void teleopInit() {
- turretPIDController = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD());
-
+ @Override
+ public void teleopInit() {
+ shoot = false;
+ countShoot = 0;
+ turretPitch = 0;
+ turretPIDController = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD());
+ hang = false;
+ countHang = 0;
+ xbox.getRawButtonPressed(3);
+ turnTo45 = false;
+ turnTo225 = false;
}
- @Override public void teleopPeriodic() {
- double jx = joystick.getX();
- double jy = -joystick.getY();
-
- //joystick deaden: yeet smol/weird joystick values when joystick is at rest
- double cutoff = 0.05;
- if(jy > -cutoff && jy < cutoff) jy = 0;
- if(jx > -cutoff && jx < cutoff) jx = 0;
+ @Override
+ public void teleopPeriodic() {
+ turretPower = 0;
+
+
+ switch (POVDirection.getDirection(xbox.getPOV())) {
+ case NORTH: //MEDIUM SHOT RANGE
+ targetRPM = 4000;
+ turretPitch = 0.38;
+ break;
+ case EAST:
+ targetRPM = 2000;
+ turretPitch = 0.8;
+ break;
+ case SOUTH: ////CLOSE SHOT--> collector out
+ targetRPM = 3400;
+ turretPitch = 0.00;
+ turnTo225 = true;
+ turnTo45 = false;
+ break;
+ case WEST:
+ targetRPM = 3600; //////////collector facing
+ turretPitch = 0.1;
+ turnTo45 = true;
+ turnTo225 = false;
+ break;
+ }
- //moved this to after joystick deaden because deaden should be focused on the raw joystick values
- double scaleFactor = 1.0;
+ if (joystick.getRawButtonPressed(8)) {
+ countHang = (countHang + 1) % 2;
+ }
- //robot PTO not on arms, give joystick carte blanche
- if(!robot.getPTOState()){
- robot.drivePercent(
- (jy+jx) * scaleFactor,
- (jy-jx) * scaleFactor
- );
+ if (countHang == 1) {
+ hang = true;
+ } else {
+ hang = false;
}
+ if (!hang) {
+ reset = true;
+ robot.turnOffPTO();
- SmartDashboard.putNumber("XBOX AXIS DEBUG - 0 ", xbox.getRawAxis(0));
- SmartDashboard.putNumber("XBOX AXIS DEBUG - 1 ", xbox.getRawAxis(1));
- SmartDashboard.putNumber("XBOX AXIS DEBUG - 2 ", xbox.getRawAxis(2));
- SmartDashboard.putNumber("XBOX AXIS DEBUG - 3 ", xbox.getRawAxis(3));
+ //////////////////////////////////////////////////DRIVETRAIN CONTROL
+ if (!robot.getPTOState()) {
+ if(joystick.getRawButton(1)){
+ driveLeft = .4;
+ driveRight = .4 ;
+ }
+ else if(joystick.getRawButton(2)){
+ driveLeft = -.4;
+ driveRight = -.4;
+ }
+ else if(joystick.getRawButton(3)){
+ //pivot left
+ driveLeft = -.4;
+ driveRight = .4;
+ }
+ else if(joystick.getRawButton(4)){
+ //pivot right
+ driveLeft = .4;
+ driveRight = -.4;
+ }
+ else{
+ joystickX = joystick.getX();
+ joystickY = -joystick.getY();
+
+ if (joystickY > -cutoff && joystickY < cutoff) {
+ joystickY = 0;
+ }
+ if (joystickX > -cutoff && joystickX < cutoff) {
+ joystickX = 0;
+ }
+
+ driveLeft = (joystickY + joystickX) * scaleFactor;
+ driveRight = (joystickY - joystickX) * scaleFactor;
+ }
- //currently Jack has no clue what axises these are supposed to be
- int leftAxis = 1; int rightAxis = 5;
- double tolerance = 0.8;
- double drivePower = 0.2;
+ }
- if (joystick.getRawButton(12)) robot.setTurretPowerPct( 0.2);
- else if (joystick.getRawButton(15)) robot.setTurretPowerPct(-0.2);
- else robot.setTurretPowerPct( 0.0);
+ //////////////////////////////////////////////DRIVETRAIN CONTROL ENDS
- double driveLeft = 0;
- double driveRight = 0;
- if(robot.getPTOState()){
- if(xbox.getRawAxis(leftAxis) > tolerance){
- driveLeft = drivePower;
+ //////////////////////////////////////////////////////////TURRET CONTROL
+
+ counter = (counter + 1) % averageTurretXSize;
+ averageX[counter] = robot.getTargetX();
+
+ average = 0;
+ for (int i = 0; i < averageTurretXSize; i++)
+ average += averageX[i];
+ average /= averageTurretXSize;
+
+ if ((xbox.getRawAxis(2) > 0.10) && robot.isTargetFound()) { ////////////AUTO-AIM
+ turnTo45 = false;
+ turnTo225 = false;
+ turretPower = turretPIDController.calculate(average);
+
}
- else if(xbox.getRawAxis(leftAxis) < -tolerance){
- driveLeft = -drivePower;
+
+ else {
+ turretPIDController.reset();
+ if (xbox.getRawButton(6)) {
+ turretPower = -0.45;
+ turnTo45 = false;
+ turnTo225 = false;
+ } else if (xbox.getRawButton(5)) {
+ turretPower = 0.45;
+ turnTo45 = false;
+ turnTo225 = false;
+ }
+ else if(turnTo45) {
+ turretPower = -turretPIDController.calculate(robot.getAlternateTurretAngle()-45);
+ }
+ else if (turnTo225){
+ turretPower = -turretPIDController.calculate(robot.getAlternateTurretAngle()-225);
+ }
+ else {
+ turretPower = 0.0;
+ }
}
- if(xbox.getRawAxis(rightAxis) > tolerance){
- driveRight = drivePower;
+
+ /////////////////////////////////////////////////////TURRET CONTROL ENDS
+
+
+ /////////////////////////////////////////////////////CLIMBER WHEN PTO
+
+ if (robot.getPTOState()) {
+ if (xbox.getRawAxis(leftAxis) > tolerance) {
+ driveLeft = defaultClimbPower;
+ } else if (xbox.getRawAxis(leftAxis) < -tolerance) {
+ driveLeft = -defaultClimbPower;
+ }
+
+ if (xbox.getRawAxis(rightAxis) > tolerance) {
+ driveRight = defaultClimbPower;
+ } else if (xbox.getRawAxis(rightAxis) < -tolerance) {
+ driveRight = -defaultClimbPower;
+ }
}
- else if(xbox.getRawAxis(rightAxis) < -tolerance){
- driveRight = -drivePower;
+ /////////////////////////////////////////////////////CLIMBER CODE ENDS
+
+
+ //////////////////////////////////////////////////////////SHOOTER CODE BEGINS
+
+ if (xbox.getRawButtonPressed(3)) {
+ countShoot = (countShoot + 1) % 2;
+ }
+ if (countShoot == 0){
+ shoot = false;
+ }
+ else{
+ shoot = true;
+ }
+ if (shoot){
+ shooterTargetRPM = targetRPM;
+ } else {
+ shooterTargetRPM = 0;
}
- robot.drivePercent(driveLeft, driveRight);
- }
+ if (xbox.getRawButton(1)) {
+ if (robot.isReadyToShoot()) {
+ robot.setActivelyShooting(true);
+ } else {
+ robot.setActivelyShooting(false);
+ }
+ } else {
+ robot.setActivelyShooting(false);
+ }
+ //force override rpm check
+ if (joystick.getRawButton(7)) {
+ robot.setActivelyShooting(true);
+ }
+ ////////////////////////////////////////////////////////////SHOOTER CODE ENDS
- //Start of Daniel+Saiarun Turret test
- average = 0;
- if(robot.isTargetFound()) {
- counter = counter % averageTurretXSize;
- averageX[counter] = robot.getTargetX();
- counter++;
- }
- average = 0;
- for(double i: averageX){
- average += i;
- }
- average /= averageTurretXSize;
- SmartDashboard.putNumber("Average", average);
+ ///////////////////////////////////////////////////////////ACTUATOR STUFF
- if (joystick.getRawButtonPressed(1)) turretPIDController.reset();
- if (joystick.getRawButton(1) && turretv !=0){
- currentTurretPower = turretPIDController.calculate(average);
- } else {
- if (joystick.getRawButton(3)) currentTurretPower = -0.1;
- else if (joystick.getRawButton(4)) currentTurretPower = 0.1;
- else currentTurretPower = 0.0;
- }
+ double deadzone = 0.5;
+ double rJoyRY = xbox.getRawAxis(5);
+ if (rJoyRY > -deadzone && rJoyRY < deadzone) rJoyRY = 0;
+
+ if (rJoyRY > deadzone) {
+ turretPitch = robot.getTurretPitchPosition() + .002;
+ }
+
+ if (rJoyRY < -deadzone) {
+ turretPitch = robot.getTurretPitchPosition() - .002;
+ }
+
+
+ /////////////////////////////////////////////////////////////////ACTUATOR STUFF ENDS
- robot.setTurretPowerPct(currentTurretPower);
+ /////////////////////////////////////////////////////////COLLECTOR CONTROLS
- //Collector indexer logic based on cargo already in sensors (from jack)
- double defCollectorPower = 1;
- double defIndexerPower = 1;
- double curCollector = 0;
- double curIndexer = 0;
- //button 2 = bottom center button
- if(joystick.getRawButton(2)){
- if(!robot.getUpperCargo()){
- curCollector = defCollectorPower;
+ //button 2 = bottom center button
+ if (xbox.getRawButton(2)) {
+ if (!robot.getUpperCargo()) {
+ //consumes ball
+ curCollector = defCollectorPower;
+ curIndexer = defIndexerPower;
+ } else {
+ curIndexer = 0;
+ if (!robot.getLowerCargo()) {
+ curCollector = defCollectorPower;
+ } else {
+ curCollector = 0;
+ }
+ }
+ if (robot.isActivelyShooting()) {
+ curIndexer = defIndexerPower;
+ }
+ } else if (xbox.getRawButton(4)) {
+ //barfs out ball
+ curCollector = -defCollectorPower;
+ curIndexer = -defIndexerPower;
+ } else {
+ curCollector = 0;
+ curIndexer = 0;
+ }
+
+ //Cargo is on upper sensor and we want to yeet it: indexer needs to push it past sensor
+ if (robot.isActivelyShooting() && robot.getUpperCargo()) {
curIndexer = defIndexerPower;
}
- else{
- curIndexer = 0;
- if(!robot.getLowerCargo()){
- curCollector = defCollectorPower;
+
+ /* Some code for setting up the collector after flight check. */
+ double flightCheckTol = 5.0;
+ double storagePosition = 317;
+
+ if (joystick.getRawButton(13)) {
+ if (robot.getAlternateTurretAngle()>storagePosition+flightCheckTol)
+ {
+ turretPower = 0.4;
}
- else{
- curCollector = 0;
+ else if (robot.getAlternateTurretAngle() 1) newPos = 1;
+
+ robot.setTurretPitchPosition(newPos);
- double driveX = joystick.getX();
+ double driveX = joystick.getX();
double driveY = -joystick.getY();
//joystick deaden: yeet smol/weird joystick values when joystick is at rest
double cutoff = 0.05;
- if(driveX > -cutoff && driveX < cutoff) driveX = 0;
- if(driveY > -cutoff && driveY < cutoff) driveY = 0;
+ if (driveX > -cutoff && driveX < cutoff) driveX = 0;
+ if (driveY > -cutoff && driveY < cutoff) driveY = 0;
//moved this to after joystick deaden because deaden should be focused on the raw joystick values
double scaleFactor = 1.0;
- robot.drivePercent(
- (driveY+driveX) * scaleFactor,
- (driveY-driveX) * scaleFactor
- );
+ if (!robot.getPTOState()) {
+ robot.drivePercent(
+ (driveY + driveX) * scaleFactor,
+ (driveY - driveX) * scaleFactor
+ );
+ }
+
+ int leftAxis = 1;
+ int rightAxis = 5;
+ double tolerance = 0.8;
+ double drivePower = 0.2;
+
+ if (joystick.getRawButton(12)) robot.setTurretPowerPct(0.2);
+ else if (joystick.getRawButton(15)) robot.setTurretPowerPct(-0.2);
+ else robot.setTurretPowerPct(0.0);
+
+ double driveLeft = 0;
+ double driveRight = 0;
+
+ if (robot.getPTOState()) {
+ if (xbox.getRawAxis(leftAxis) > tolerance) {
+ driveLeft = drivePower;
+ } else if (xbox.getRawAxis(leftAxis) < -tolerance) {
+ driveLeft = -drivePower;
+ }
+
+ if (xbox.getRawAxis(rightAxis) > tolerance) {
+ driveRight = drivePower;
+ } else if (xbox.getRawAxis(rightAxis) < -tolerance) {
+ driveRight = -drivePower;
+ }
+ robot.drivePercent(driveLeft, driveRight);
+ }
//note to self: buttons control mirrored joystick setting
- if(joystick.getRawButton(11)) {
+ if (joystick.getRawButton(11)) {
robot.setCollectorIntakePercentage(1);
robot.setIndexerIntakePercentage(1);
- }
- else if(joystick.getRawButton(16)){
+ } else if (joystick.getRawButton(16)) {
robot.setCollectorIntakePercentage(-1);
robot.setIndexerIntakePercentage(-1);
- }
- else{
+ } else {
robot.setCollectorIntakePercentage(0);
robot.setIndexerIntakePercentage(0);
}
- if (joystick.getRawButton(12)) robot.setTurretPowerPct( 0.2);
+ if (joystick.getRawButton(12)) robot.setTurretPowerPct(0.2);
else if (joystick.getRawButton(15)) robot.setTurretPowerPct(-0.2);
- else robot.setTurretPowerPct( 0.0);
+ else robot.setTurretPowerPct(0.0);
- if (joystick.getRawButton(13)){
- robot.setShooterPowerPct( 0.2, 0.2);
+ if (joystick.getRawButton(13)) {
+ robot.setShooterPowerPct(0.2, 0.2);
robot.setActivelyShooting(true);
- }
- else if (joystick.getRawButton(14)){
+ } else if (joystick.getRawButton(14)) {
robot.setShooterPowerPct(-0.2, -0.2);
robot.setActivelyShooting(false);
- }
- else {
- robot.setShooterPowerPct( 0.0, 0.0);
+ } else {
+ robot.setShooterPowerPct(0.0, 0.0);
robot.setActivelyShooting(false);
}
- if (joystick.getRawButton( 7)) robot.raiseCollector();
- if (joystick.getRawButton( 8)) robot.lowerCollector();
+ if (joystick.getRawButton(7)) robot.raiseCollector();
+ if (joystick.getRawButton(8)) robot.lowerCollector();
+
+ if (joystick.getRawButton(6)) robot.turnOnPTO();
+ if (joystick.getRawButton(9)) robot.turnOffPTO();
+
+ if (joystick.getRawButton(5)) robot.setArmsForward();
+ if (joystick.getRawButton(10)) robot.setArmsBackward();
+
+ if (joystick.getRawButtonPressed(4)){
+ countHang = (countHang + 1) % 2;
+ }
+
+ if (countHang == 1) {
+ hang = true;
+ } else {
+ hang = false;
+ }
+
+ if (hang){
+ if (reset) {
+ testHang.begin(robot);
+ reset = false;
+ }
+ testHang.step(robot);
+ }
+ else{
+ reset = true;
+ }
- if (joystick.getRawButton( 6)) robot.turnOnPTO();
- if (joystick.getRawButton( 9)) robot.turnOffPTO();
- if (joystick.getRawButton( 5)) robot.setArmsForward();
- if (joystick.getRawButton(10)) robot.setArmsBackward();
+ }
+ public enum POVDirection {
+ NORTH(0),
+ NORTHEAST(45),
+ EAST(90),
+ SOUTHEAST(135),
+ SOUTH(180),
+ SOUTHWEST(225),
+ WEST(270), //best
+ NORTHWEST(315),
+ NULL(-1);
+
+ private final int angle;
+
+ POVDirection(int angle) {
+ this.angle = angle;
+ }
+
+ public int getAngle() {
+ return angle;
+ }
+
+ //Kevin voodoo to turn ints into directions
+ public static final Map directionMap =
+ Arrays.stream(POVDirection.values()).collect(
+ Collectors.toMap(
+ POVDirection::getAngle,
+ Function.identity()
+ )
+ );
+
+ public static POVDirection getDirection(int angle) {
+ return directionMap.getOrDefault(angle, POVDirection.NULL);
+ }
}
-}
\ No newline at end of file
+}
diff --git a/src/main/java/frc/robot/autonomous/SimpleATerminal.java b/src/main/java/frc/robot/autonomous/BallAtoB.java
similarity index 63%
rename from src/main/java/frc/robot/autonomous/SimpleATerminal.java
rename to src/main/java/frc/robot/autonomous/BallAtoB.java
index fa6b624..ebfc432 100644
--- a/src/main/java/frc/robot/autonomous/SimpleATerminal.java
+++ b/src/main/java/frc/robot/autonomous/BallAtoB.java
@@ -1,14 +1,11 @@
package frc.robot.autonomous;
import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.generic.GenericRobot;
//Simple autonomous code for ball A, closest ball to the scoring table, and driving to the ball at terminal
-public class SimpleATerminal extends GenericAutonomous {
+public class BallAtoB extends GenericAutonomous {
double startingYaw;
double leftpower;
@@ -20,50 +17,25 @@ public class SimpleATerminal extends GenericAutonomous {
double startTime;
double startDistance;
- double distanceA = 37;
- double distanceB = 259.26;
+ double distanceA = 40.44;
+ double distanceB = 259.26; //CHANGE
double angleA = 87.74;
-
+ double angleB = 0; //change
double rampDownDist = 10;
PIDController PIDDriveStraight;
- //
- int averageTurretXSize = 2;
- double[] averageTurretX = new double [averageTurretXSize];
- double turretx;
- double turrety;
- double turretarea;
- double turretv;
- int counter = 0;
- PIDController turretPIDController;
- //
-
@Override
public void autonomousInit(GenericRobot robot) {
autonomousStep = 0;
- startingYaw = robot.getYaw(); //might need to change to set degrees
+ startingYaw = robot.getYaw();
startTime = System.currentTimeMillis();
PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDpivotD());
-
- turretPIDController = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD());
}
@Override
public void autonomousPeriodic(GenericRobot robot) {
- //
- NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
- NetworkTableEntry tx = table.getEntry("tx");
- NetworkTableEntry ty = table.getEntry("ty");
- NetworkTableEntry ta = table.getEntry("ta");
- NetworkTableEntry tv = table.getEntry("tv");
-
- turretx = tx.getDouble(0.0);
- turrety = ty.getDouble(0.0);
- turretarea = ta.getDouble(0.0);
- turretv = tv.getDouble(0.0);
- //
switch(autonomousStep){
case 0: //reset
@@ -88,14 +60,12 @@ public void autonomousPeriodic(GenericRobot robot) {
rightpower = defaultPower - correction;
if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceA - rampDownDist){
- double a = rampDown(defaultPower, 0, startDistance, rampDownDist, robot.getDriveDistanceInchesLeft(), distanceA);
- leftpower = a;
- rightpower = a;
+ defaultPower = (distanceA-robot.getDriveDistanceInchesLeft()+startDistance)*defaultPower/rampDownDist;
}
if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceA){
autonomousStep += 1;
startTime = System.currentTimeMillis();
- } //has 3 inches of momentum with .25 power
+ }
break;
case 5: //stop
leftpower = 0;
@@ -131,15 +101,13 @@ public void autonomousPeriodic(GenericRobot robot) {
rightpower = defaultPower - correction;
if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceB - rampDownDist){
- double a = rampDown(defaultPower, 0, startDistance, rampDownDist, robot.getDriveDistanceInchesLeft(), distanceB);
- leftpower = a;
- rightpower = a;
+ defaultPower = (distanceB-robot.getDriveDistanceInchesLeft()+startDistance)*defaultPower/rampDownDist;
}
if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceB) {
autonomousStep += 1;
leftpower = 0;
rightpower = 0;
- } //might need to tune for momentum
+ }
break;
case 14:
leftpower = 0;
@@ -148,27 +116,5 @@ public void autonomousPeriodic(GenericRobot robot) {
}
robot.drivePercent(leftpower, rightpower);
- //If turret works set value of averageTurretX[] to turretx
- if(turretv !=0 ) {
- averageTurretX[counter % averageTurretXSize] = turretx;
- counter++;
- }
-
- double average = 0;
- for(double i: averageTurretX){
- average += i;
- }
- average /= averageTurretXSize;
-
- double currentTurretPower = 0;
-
- if(turretv !=0){
- currentTurretPower = turretPIDController.calculate(average);
- }else{
- turretPIDController.reset();
- }
-
- robot.setTurretPowerPct(currentTurretPower);
-
}
}
diff --git a/src/main/java/frc/robot/autonomous/BallAtoBtoTerminal.java b/src/main/java/frc/robot/autonomous/BallAtoBtoTerminal.java
new file mode 100644
index 0000000..c6c2041
--- /dev/null
+++ b/src/main/java/frc/robot/autonomous/BallAtoBtoTerminal.java
@@ -0,0 +1,152 @@
+package frc.robot.autonomous;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.generic.GenericRobot;
+
+//Simple autonomous code for ball A, and then driving to the ball B, and finally to ball at terminal
+public class BallAtoBtoTerminal extends GenericAutonomous {
+ double startingYaw;
+ double startTime;
+ double startDistance;
+
+ double leftpower;
+ double rightpower;
+ double defaultPower = .4;
+ double defaultTurnPower = .4;
+ double correction;
+
+ double distanceA = 37;
+ double distanceB = 117.1;
+ double distanceTerminal = 160.6;
+ double angleA = 87.74;
+ double angleB = 0; //ASK CAD LATER
+ double rampDownDist = 10;
+
+ PIDController PIDDriveStraight;
+
+ @Override
+ public void autonomousInit(GenericRobot robot) {
+ autonomousStep = 0;
+ startingYaw = robot.getYaw();
+ startTime = System.currentTimeMillis();
+ PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDpivotD());
+ }
+
+ @Override
+ public void autonomousPeriodic(GenericRobot robot) {
+
+ switch(autonomousStep){
+ case 0: //reset
+ robot.lowerCollector();
+ PIDDriveStraight.reset();
+ PIDDriveStraight.enableContinuousInput(-180,180);
+ robot.resetEncoders();
+ if (System.currentTimeMillis() - startTime > 100){
+ autonomousStep = 4;
+ startingYaw = robot.getYaw();
+ startDistance = robot.getDriveDistanceInchesLeft();
+ }
+ break;
+ case 1: //shoot the ball
+ case 2: //shoot the ball part 2 electric boogaloo
+ case 3: //shoot the ball part 3 maybe
+ case 4: //drive to ball A
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceA - rampDownDist){
+ defaultPower = (distanceA-robot.getDriveDistanceInchesLeft()+startDistance)*defaultPower/rampDownDist;
+ }
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceA){
+ autonomousStep += 1;
+ startTime = System.currentTimeMillis();
+ }
+ break;
+ case 5: //stop
+ leftpower = 0;
+ rightpower = 0;
+ if (System.currentTimeMillis() - startTime > 1000){
+ autonomousStep = 12;
+ }
+ break;
+ case 6: //collector to collect ball
+ case 7: //collection part 2 not electric nor boogaloo
+ case 8: //nother collection case
+ case 9: //shoot the second ball for funsies
+ case 10: //miss the target and become sadge
+ case 11: //copium
+ //will change these comments when they actually mean something
+ case 12: //turn to go to ball B
+ leftpower = defaultTurnPower;
+ rightpower = -defaultTurnPower;
+ //turning right
+
+ if(robot.getYaw() - startingYaw > angleA) {
+ startingYaw = startingYaw + angleA;
+ startDistance = robot.getDriveDistanceInchesLeft();
+ PIDDriveStraight.reset();
+ autonomousStep += 1;
+ }
+ break;
+ case 13: //drive towards the ball B
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceB - rampDownDist){
+ defaultPower = (distanceB-robot.getDriveDistanceInchesLeft()+startDistance)*defaultPower/rampDownDist;
+ }
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceB) {
+ autonomousStep += 1;
+ leftpower = 0;
+ rightpower = 0;
+ }
+ break;
+ case 14: //collect / shoot
+ case 15:
+ case 16:
+ case 17:
+ case 18:
+ case 19:
+ case 20:
+ case 21: //turn to ball Terminal
+ leftpower = -defaultTurnPower;
+ rightpower = defaultTurnPower;
+ //turning ???
+
+ if(robot.getYaw() - startingYaw > angleB) {
+ startingYaw = robot.getYaw();
+ startDistance = robot.getDriveDistanceInchesLeft();
+ autonomousStep += 1;
+ }
+ break;
+ case 22: //drive to ball Terminal
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal - rampDownDist){
+ defaultPower = (distanceTerminal -robot.getDriveDistanceInchesLeft()+startDistance)*defaultPower/rampDownDist;
+ }
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal) {
+ autonomousStep += 1;
+ leftpower = 0;
+ rightpower = 0;
+ }
+ break;
+ case 23:
+ leftpower = 0;
+ rightpower = 0;
+ break;
+ case 24:
+ case 25:
+ }
+ robot.drivePercent(leftpower, rightpower);
+
+ }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/autonomous/BallAtoTerminal.java b/src/main/java/frc/robot/autonomous/BallAtoTerminal.java
new file mode 100644
index 0000000..a2db35f
--- /dev/null
+++ b/src/main/java/frc/robot/autonomous/BallAtoTerminal.java
@@ -0,0 +1,159 @@
+package frc.robot.autonomous;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.generic.GenericRobot;
+
+//Simple autonomous code for ball A, closest ball to the scoring table, and driving to the ball at terminal
+public class BallAtoTerminal extends GenericAutonomous {
+ double startingYaw;
+ double startTime;
+ double startDistance;
+
+ double leftpower;
+ double rightpower;
+ double defaultPower = .4;
+ double correction;
+ boolean time = false;
+ boolean readyToShoot = false;
+ double shooterTargetRPM;
+
+ double distanceA = 44.2;
+ double distanceTerminal = 259.26;
+ double angleA = 88.74;
+ double rampDownDist = 10;
+
+ PIDController PIDDriveStraight;
+ PIDController PIDPivot;
+ TurretTracker tracker = new TurretTracker();
+
+ @Override
+ public void autonomousInit(GenericRobot robot) {
+ autonomousStep = 0;
+ startingYaw = robot.getYaw();
+ startTime = System.currentTimeMillis();
+ shooterTargetRPM = 0;
+ readyToShoot = false;
+ PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDpivotD());
+ PIDPivot = new PIDController(robot.getPIDpivotP(), robot.getPIDpivotI(), robot.getPIDpivotD());
+ tracker.turretInit(robot);
+ }
+
+ @Override
+ public void autonomousPeriodic(GenericRobot robot) {
+ tracker.turretUpdate(robot);
+
+ if (autonomousStep >= 1){
+ robot.getCargo();
+ robot.shoot();
+ robot.setTurretPitchPosition(.38);
+ }
+ switch(autonomousStep){
+ case 0: //reset
+ robot.lowerCollector();
+ PIDDriveStraight.reset();
+ PIDPivot.reset();
+ PIDDriveStraight.enableContinuousInput(-180,180);
+ PIDPivot.enableContinuousInput(-180,180);
+ robot.resetEncoders();
+ robot.resetAttitude();
+ if (System.currentTimeMillis() - startTime > 100){
+ autonomousStep += 1;
+ startingYaw = robot.getYaw();
+ startDistance = robot.getDriveDistanceInchesLeft();
+ }
+ break;
+ case 1: //drive to ball A
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceA - rampDownDist){
+ double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist,
+ robot.getDriveDistanceInchesLeft(), distanceA);
+ leftpower = ramp;
+ rightpower = ramp;
+ }
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceA){
+ autonomousStep += 1;
+ startTime = System.currentTimeMillis();
+ }
+ break;
+ case 2: //stop
+ leftpower = 0;
+ rightpower = 0;
+ if (System.currentTimeMillis() - startTime > 1000){
+ autonomousStep += 1;
+ }
+ break;
+ case 3: //shoot the ball if target is found
+ if (robot.canShoot()){
+ robot.setActivelyShooting(true);
+ startTime = System.currentTimeMillis();
+ autonomousStep += 1;
+ }
+ break;
+ case 4: //turn shooter off
+ if (System.currentTimeMillis()-startTime >= 1000){
+ robot.setActivelyShooting(false);
+ autonomousStep += 1;
+ }
+ break;
+ case 5: //reset
+ autonomousStep += 1;
+ break;
+ case 6: //turn to go to ball @ terminal
+ correction = PIDPivot.calculate(-angleA + robot.getYaw() - startingYaw);
+ leftpower = correction;
+ rightpower = -correction;
+ //turning right
+ if (Math.abs(Math.abs(robot.getYaw() - startingYaw)-angleA) <= 1.5){
+ if (!time){
+ startTime = System.currentTimeMillis();
+ time = true;
+ }
+ }
+ else{
+ startTime = System.currentTimeMillis();
+ time = false;
+ }
+ if (System.currentTimeMillis() - startTime >= 50){
+ leftpower = 0;
+ rightpower = 0;
+ autonomousStep += 1;
+ startingYaw = -angleA;
+ startDistance = robot.getDriveDistanceInchesLeft();
+ startTime = System.currentTimeMillis();
+ }
+ break;
+
+ case 7: //drive towards the ball
+ //SKIPPED RIGHT NOW
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal - rampDownDist){
+ double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist,
+ robot.getDriveDistanceInchesLeft(), distanceTerminal);
+ leftpower = ramp;
+ rightpower = ramp;
+ }
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal) {
+ autonomousStep += 1;
+ leftpower = 0;
+ rightpower = 0;
+ }
+ break;
+ case 8:
+ leftpower = 0;
+ rightpower = 0;
+ break;
+ }
+ robot.drivePercent(leftpower, rightpower);
+ tracker.turretMove(robot);
+
+ }
+}
diff --git a/src/main/java/frc/robot/autonomous/BallAtoTerminalReturn.java b/src/main/java/frc/robot/autonomous/BallAtoTerminalReturn.java
new file mode 100644
index 0000000..589e60e
--- /dev/null
+++ b/src/main/java/frc/robot/autonomous/BallAtoTerminalReturn.java
@@ -0,0 +1,234 @@
+package frc.robot.autonomous;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.generic.GenericRobot;
+
+//Simple autonomous code for ball A, closest ball to the scoring table, and driving to the ball at terminal
+//Setup: Line the robot straight on the tape facing ball A
+public class BallAtoTerminalReturn extends GenericAutonomous {
+ double startingYaw;
+ double startTime;
+ double startDistance;
+
+ double leftpower;
+ double rightpower;
+ double defaultPower = .4;
+ double correction;
+ boolean time = false;
+ boolean readyToShoot = false;
+ double shooterTargetRPM;
+
+ int averageTurretXSize = 2;
+ double[] averageTurretX = new double [averageTurretXSize];
+ int counter = 0;
+
+ double distanceA = 44.2;
+ double distanceTerminal = 259.26;
+ double angleA = 88.74;
+ double rampDownDist = 10;
+
+ PIDController PIDDriveStraight;
+ PIDController PIDPivot;
+ PIDController turretPIDController;
+ @Override
+ public void autonomousInit(GenericRobot robot) {
+ autonomousStep = 0;
+ startingYaw = robot.getYaw();
+ startTime = System.currentTimeMillis();
+ shooterTargetRPM = 0;
+ readyToShoot = false;
+ PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDpivotD());
+ PIDPivot = new PIDController(robot.getPIDpivotP(), robot.getPIDpivotI(), robot.getPIDpivotD());
+ turretPIDController = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD());
+ robot.setPipeline(0);
+ }
+
+ @Override
+ public void autonomousPeriodic(GenericRobot robot) {
+ double currentTurretPower = 0;
+ double average = 0;
+
+ if(robot.isTargetFound()) {
+ averageTurretX[counter % averageTurretXSize] = robot.getTargetX();
+ counter++;
+ }
+ for(double i: averageTurretX){
+ average += i;
+ }
+ average /= averageTurretXSize;
+
+ currentTurretPower = turretPIDController.calculate(average);
+
+ if (autonomousStep < 4){
+ if((!robot.isTargetFound()) && (System.currentTimeMillis() - startTime < 5000)) {
+ currentTurretPower = .3;
+ }
+ }
+ if ((autonomousStep>=4) && (autonomousStep < 10)){
+ if((!robot.isTargetFound()) && (System.currentTimeMillis() - startTime < 5000)) {
+ currentTurretPower = .2;
+ }
+ }
+
+
+ if (autonomousStep >= 1){
+ robot.getCargo();
+ robot.shoot();
+ robot.setShooterTargetRPM(3700);
+ }
+ if (autonomousStep >= 1 && autonomousStep <=10){
+ robot.setTurretPitchPosition(.38);
+ } else{
+ robot.setCollectorIntakePercentage(0);
+ currentTurretPower = 0;
+ }
+
+ robot.setTurretPowerPct(currentTurretPower);
+
+ switch(autonomousStep){
+ case 0: //reset
+ robot.lowerCollector();
+ PIDDriveStraight.reset();
+ PIDPivot.reset();
+ PIDDriveStraight.enableContinuousInput(-180,180);
+ PIDPivot.enableContinuousInput(-180,180);
+ robot.resetEncoders();
+ robot.resetAttitude();
+ if (System.currentTimeMillis() - startTime > 500){
+ autonomousStep += 1;
+ startingYaw = robot.getYaw();
+ startDistance = robot.getDriveDistanceInchesLeft();
+ }
+ break;
+ case 1: //drive to ball A
+
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceA - rampDownDist){
+ double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist,
+ robot.getDriveDistanceInchesLeft(), distanceA);
+ leftpower = ramp;
+ rightpower = ramp;
+ }
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceA){
+ autonomousStep += 1;
+ startTime = System.currentTimeMillis();
+ }
+ break;
+ case 2: //stop
+ leftpower = 0;
+ rightpower = 0;
+ startDistance = robot.getDriveDistanceInchesLeft();
+ autonomousStep += 1;
+ time = false;
+ break;
+ case 3: //shoot the ball if target is found
+ if (robot.isTargetFound() && robot.canShoot() && (-5 < average) && (average< 5)){
+ robot.setActivelyShooting(true);
+ startTime = System.currentTimeMillis();
+ autonomousStep += 1;
+ }
+ break;
+ case 4: //turn shooter off
+ if (System.currentTimeMillis()-startTime >= 2000){
+ robot.setActivelyShooting(false);
+ autonomousStep += 1;
+ }
+ break;
+ case 5: //reset
+ PIDDriveStraight.reset();
+ PIDDriveStraight.enableContinuousInput(-180,180);
+ startDistance = robot.getDriveDistanceInchesLeft();
+ autonomousStep +=1;
+ break;
+ case 6: //turn to go to ball @ terminal
+ correction = PIDPivot.calculate(-angleA + robot.getYaw() - startingYaw);
+ leftpower = correction;
+ rightpower = -correction;
+ robot.setPipeline(1);
+ //turning right
+ if (Math.abs(Math.abs(robot.getYaw() - startingYaw)-angleA) <= 1.5){
+ if (!time){
+ startTime = System.currentTimeMillis();
+ time = true;
+ }
+ }
+ else{
+ startTime = System.currentTimeMillis();
+ time = false;
+ }
+ if (System.currentTimeMillis() - startTime >= 50){
+ leftpower = 0;
+ rightpower = 0;
+ autonomousStep += 1;
+ startingYaw = -angleA;
+ startDistance = robot.getDriveDistanceInchesLeft();
+ startTime = System.currentTimeMillis();
+ }
+ break;
+ case 7: //drive towards Ball Terminal
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal - rampDownDist){
+ double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist,
+ robot.getDriveDistanceInchesLeft(), distanceTerminal);
+ leftpower = ramp;
+ rightpower = ramp;
+ }
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal) {
+ autonomousStep += 1;
+ startDistance = robot.getDriveDistanceInchesLeft();
+ startTime = System.currentTimeMillis();
+ }
+ break;
+ case 8: //collect ball at Terminal
+ leftpower = 0;
+ rightpower = 0;
+ autonomousStep += 1;
+ robot.setPipeline(0);
+ case 9: // drive back
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = -1*(defaultPower + correction);
+ rightpower = -1*(defaultPower - correction);
+
+ if(Math.abs(robot.getDriveDistanceInchesLeft() - startDistance) >= distanceTerminal - rampDownDist){
+ double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist,
+ robot.getDriveDistanceInchesLeft(), distanceTerminal);
+ leftpower = -ramp;
+ rightpower = -ramp;
+ }
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal) {
+ autonomousStep += 1;
+ leftpower = 0;
+ rightpower = 0;
+ }
+ break;
+ case 10: //shoot the ball if target is found
+ if (robot.canShoot()){
+ robot.setActivelyShooting(true);
+ startTime = System.currentTimeMillis();
+ autonomousStep += 1.0;
+ }
+ break;
+ case 11: //turn shooter off
+ if (System.currentTimeMillis()-startTime >= 1000){
+ robot.setActivelyShooting(false);
+ autonomousStep += 1;
+ }
+ break;
+ case 12:
+ leftpower = 0;
+ rightpower = 0;
+ break;
+ }
+ robot.drivePercent(leftpower, rightpower);
+ }
+}
diff --git a/src/main/java/frc/robot/autonomous/SimpleBTerminal.java b/src/main/java/frc/robot/autonomous/BallBtoA.java
similarity index 99%
rename from src/main/java/frc/robot/autonomous/SimpleBTerminal.java
rename to src/main/java/frc/robot/autonomous/BallBtoA.java
index c04ed21..870308e 100644
--- a/src/main/java/frc/robot/autonomous/SimpleBTerminal.java
+++ b/src/main/java/frc/robot/autonomous/BallBtoA.java
@@ -8,7 +8,7 @@
import frc.robot.generic.GenericRobot;
//Simple autonomous code for ball A, closest ball to the hangar
-public class SimpleBTerminal extends GenericAutonomous {
+public class BallBtoA extends GenericAutonomous {
double startingYaw;
double leftpower;
double rightpower;
diff --git a/src/main/java/frc/robot/autonomous/BallBtoC.java b/src/main/java/frc/robot/autonomous/BallBtoC.java
new file mode 100644
index 0000000..23d33b2
--- /dev/null
+++ b/src/main/java/frc/robot/autonomous/BallBtoC.java
@@ -0,0 +1,100 @@
+package frc.robot.autonomous;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.generic.GenericRobot;
+
+//Simple autonomous code for ball A, closest ball to the hangar
+public class BallBtoC extends GenericAutonomous {
+ double startingYaw;
+ double leftpower;
+ double rightpower;
+ double defaultPower = .4;
+
+ double correction;
+ double startDistance;
+ double startTime;
+
+ PIDController PIDDriveStraight;
+
+ @Override
+ public void autonomousInit(GenericRobot robot) {
+ autonomousStep = 0;
+ startingYaw = robot.getYaw(); //might need to change to set degrees
+ PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD());
+ startTime = System.currentTimeMillis();
+ }
+
+ @Override
+ public void autonomousPeriodic(GenericRobot robot) {
+
+ switch(autonomousStep){
+ case 0: //reset
+ robot.lowerCollector();
+ PIDDriveStraight.reset();
+ PIDDriveStraight.enableContinuousInput(-180,180);
+ robot.resetEncoders();
+ robot.resetAttitude();
+ if (System.currentTimeMillis() - startTime > 100){
+ startDistance = robot.getDriveDistanceInchesLeft();
+ startingYaw = robot.getYaw();
+ autonomousStep = 4;
+ }
+ break;
+ case 1: //shoot the ball
+ case 2: //shoot the ball part 2 electric boogaloo
+ case 3: //shoot the ball part 3 maybe
+ case 4: //drive to ball A
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if(robot.getDriveDistanceInchesLeft() >= 61.5){
+ autonomousStep += 1;
+ startTime = System.currentTimeMillis();
+ } //has 3 inches of momentum with .25 power
+ break;
+ case 5: //stop
+ leftpower = 0;
+ rightpower = 0;
+ autonomousStep = 12;
+ break;
+ case 6: //collector to collect ball
+ case 7: //collection part 2 not electric nor boogaloo
+ case 8: //nother collection case
+ case 9: //shoot the second ball for funsies
+ case 10: //miss the target and become sadge
+ case 11: //copium
+ //will change these comments when they actually mean something
+ case 12://reset
+ if (System.currentTimeMillis() - startTime >= 1000){
+ PIDDriveStraight.reset();
+ PIDDriveStraight.enableContinuousInput(-180,180);
+ startDistance = robot.getDriveDistanceInchesLeft();
+ autonomousStep +=1;
+ }
+ break;
+ case 13://drive to ball at terminal
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= 153.5){
+ autonomousStep += 1;
+ }
+ break;
+ case 14:
+ leftpower = 0;
+ rightpower = 0;
+ break;
+ case 15: //collector to collect ball
+ case 16: //collection part 2 not electric nor boogaloo
+ case 17: //nother collection case
+ case 18: //shoot the second ball for funsies
+ }
+ robot.drivePercent(leftpower, rightpower);
+
+ }
+}
diff --git a/src/main/java/frc/robot/autonomous/BallBtoTerminal.java b/src/main/java/frc/robot/autonomous/BallBtoTerminal.java
new file mode 100644
index 0000000..032b0e5
--- /dev/null
+++ b/src/main/java/frc/robot/autonomous/BallBtoTerminal.java
@@ -0,0 +1,154 @@
+package frc.robot.autonomous;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.generic.GenericRobot;
+
+//Simple autonomous code for ball B, ball between ball A and C
+//Setup: put the robot down so that the ball at the terminal and the ball B are lined up straight.
+public class BallBtoTerminal extends GenericAutonomous {
+ double startingYaw;
+ double startDistance;
+ double startTime;
+
+ double leftpower;
+ double rightpower;
+ double defaultPower = .4;
+ double correction;
+
+ double distanceB = 61.5;
+ double distanceTerminal = 160.6;
+ double rampDownDist = 10;
+
+ PIDController PIDTurret;
+
+ int averageTurretXSize = 2;
+ double[] averageTurretX = new double [averageTurretXSize];
+
+ int counter = 0;
+
+ PIDController PIDDriveStraight;
+
+ @Override
+ public void autonomousInit(GenericRobot robot) {
+ autonomousStep = 0;
+ startingYaw = robot.getYaw();
+ PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD());
+ startTime = System.currentTimeMillis();
+ PIDTurret = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD());
+ }
+
+ @Override
+ public void autonomousPeriodic(GenericRobot robot) {
+
+ if (autonomousStep >= 1){
+ robot.getCargo();
+ robot.shoot();
+ robot.setTurretPitchPosition(.38);
+ }
+ switch(autonomousStep){
+ case 0: //reset
+ robot.lowerCollector();
+ PIDDriveStraight.reset();
+ PIDDriveStraight.enableContinuousInput(-180,180);
+ robot.resetEncoders();
+ robot.resetAttitude();
+ if (System.currentTimeMillis() - startTime > 100){
+ startDistance = robot.getDriveDistanceInchesLeft();
+ startingYaw = robot.getYaw();
+ autonomousStep += 1;
+ }
+ break;
+ case 1: //drive to ball B
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceB - rampDownDist){
+ double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist,
+ robot.getDriveDistanceInchesLeft(), distanceB);
+ leftpower = ramp;
+ rightpower = ramp;
+ }
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceB){
+ autonomousStep += 1;
+ startTime = System.currentTimeMillis();
+ }
+ break;
+ case 2: //stop
+ leftpower = 0;
+ rightpower = 0;
+ autonomousStep += 1;
+ break;
+ case 3:
+ if (robot.canShoot()){
+ robot.setActivelyShooting(true);
+ startTime = System.currentTimeMillis();
+ autonomousStep += 1.0;
+ }
+ break;
+ case 4: // part 2 not electric nor boogaloo
+ if (System.currentTimeMillis() - startTime >= 1000){
+ robot.setActivelyShooting(false);
+ autonomousStep += 1;
+ }
+ break;
+
+ case 5://reset
+
+ PIDDriveStraight.reset();
+ PIDDriveStraight.enableContinuousInput(-180,180);
+ startDistance = robot.getDriveDistanceInchesLeft();
+ autonomousStep +=1;
+
+ break;
+ case 6://drive to ball at terminal
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal - rampDownDist){
+ double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist, robot.getDriveDistanceInchesLeft(), distanceTerminal);
+ leftpower = ramp;
+ rightpower = ramp;
+ }
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal) {
+ autonomousStep += 1;
+ leftpower = 0;
+ rightpower = 0;
+ }
+ break;
+ case 7:
+ leftpower = 0;
+ rightpower = 0;
+ break;
+ }
+ robot.drivePercent(leftpower, rightpower);
+
+ if(robot.isTargetFound()) {
+ averageTurretX[counter % averageTurretXSize] = robot.getTargetX();
+ counter++;
+ }
+
+ double average = 0;
+ for(double i: averageTurretX){
+ average += i;
+ }
+ average /= averageTurretXSize;
+
+ double currentTurretPower = 0;
+
+ if(robot.isTargetFound()){
+ currentTurretPower = PIDTurret.calculate(average);
+ }else{
+ PIDTurret.reset();
+ }
+ if((!robot.isTargetFound()) && (System.currentTimeMillis() - startTime < 2000)) {
+ currentTurretPower = .2;
+ }
+ robot.setTurretPowerPct(currentTurretPower);
+
+ }
+}
diff --git a/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java b/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java
new file mode 100644
index 0000000..edf92fa
--- /dev/null
+++ b/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java
@@ -0,0 +1,201 @@
+package frc.robot.autonomous;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.generic.GenericRobot;
+
+//Simple autonomous code for ball B, ball between ball A and C
+//Setup: put the robot down so that the ball at the terminal and the ball B are lined up straight.
+public class BallBtoTerminalReturn extends GenericAutonomous {
+ double startingYaw;
+ double startDistance;
+ double startTime;
+
+ double leftpower;
+ double rightpower;
+ double defaultPower = .5;
+ double correction;
+
+ double distanceB = 61.5;
+ double distanceTerminal = 142.6;
+ double rampDownDist = 10;
+
+ PIDController PIDTurret;
+
+ int averageTurretXSize = 2;
+ double[] averageTurretX = new double [averageTurretXSize];
+
+ int counter = 0;
+
+ PIDController PIDDriveStraight;
+
+ @Override
+ public void autonomousInit(GenericRobot robot) {
+ autonomousStep = 0;
+ startingYaw = robot.getYaw();
+ PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD());
+ startTime = System.currentTimeMillis();
+ PIDTurret = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD());
+ }
+
+ @Override
+ public void autonomousPeriodic(GenericRobot robot) {
+
+ if(robot.isTargetFound()) {
+ averageTurretX[counter % averageTurretXSize] = robot.getTargetX();
+ counter++;
+ }
+
+ double average = 0;
+ for(double i: averageTurretX){
+ average += i;
+ }
+ average /= averageTurretXSize;
+
+ double currentTurretPower = 0;
+
+ if(robot.isTargetFound()){
+ currentTurretPower = PIDTurret.calculate(average);
+ }else{
+ PIDTurret.reset();
+ }
+ if((!robot.isTargetFound()) && (System.currentTimeMillis() - startTime < 2000)) {
+ currentTurretPower = .4;
+ }
+ robot.setTurretPowerPct(currentTurretPower);
+ //////////AUTO TRACK STUFF
+
+
+ if (autonomousStep >= 1){
+ robot.getCargo();
+ robot.shoot();
+ robot.setShooterTargetRPM(3700);
+ }
+ if (autonomousStep >= 1 && autonomousStep <=10){
+ robot.setTurretPitchPosition(.38);
+ }
+ else{
+ robot.setCollectorIntakePercentage(0);
+ robot.setTurretPowerPct(0);
+ }
+ switch(autonomousStep){
+ case 0: //reset
+ robot.lowerCollector();
+ PIDDriveStraight.reset();
+ PIDDriveStraight.enableContinuousInput(-180,180);
+ robot.resetEncoders();
+ robot.resetAttitude();
+ if (System.currentTimeMillis() - startTime > 100){
+ startDistance = robot.getDriveDistanceInchesLeft();
+ startingYaw = robot.getYaw();
+ autonomousStep += 1;
+ }
+ break;
+
+ case 1: //drive to ball B
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceB - rampDownDist){
+ double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist,
+ robot.getDriveDistanceInchesLeft(), distanceB);
+ leftpower = ramp;
+ rightpower = ramp;
+ }
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceB){
+ autonomousStep += 1;
+ startTime = System.currentTimeMillis();
+ }
+ break;
+ case 2: //stop
+ leftpower = 0;
+ rightpower = 0;
+ autonomousStep += 1;
+ break;
+ case 3:
+ if (robot.isTargetFound() && robot.canShoot() && (-5 < average) && (average < 5)){
+ robot.setActivelyShooting(true);
+ startTime = System.currentTimeMillis();
+ autonomousStep += 1.0;
+ }
+ break;
+ case 4: // part 2 not electric nor boogaloo
+ if (System.currentTimeMillis() - startTime >= 2000){
+ robot.setActivelyShooting(false);
+ autonomousStep += 1;
+ }
+ break;
+
+ case 5://reset
+
+ PIDDriveStraight.reset();
+ PIDDriveStraight.enableContinuousInput(-180,180);
+ startDistance = robot.getDriveDistanceInchesLeft();
+ autonomousStep +=1;
+
+ break;
+ case 6://drive to ball at terminal
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal - rampDownDist){
+ double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist, robot.getDriveDistanceInchesLeft(), distanceTerminal);
+ leftpower = ramp;
+ rightpower = ramp;
+ }
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal) {
+ autonomousStep += 1;
+ leftpower = 0;
+ rightpower = 0;
+ }
+ break;
+ case 7:
+ leftpower = 0;
+ rightpower = 0;
+ startDistance = robot.getDriveDistanceInchesLeft();
+ autonomousStep += 1.0;
+ break;
+ case 8:
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = -defaultPower + correction;
+ rightpower = -defaultPower - correction;
+
+ if(Math.abs(robot.getDriveDistanceInchesLeft() - startDistance) >= distanceTerminal - rampDownDist){
+ double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist, robot.getDriveDistanceInchesLeft(), distanceTerminal);
+ leftpower = -ramp;
+ rightpower = -ramp;
+ }
+ if(Math.abs(robot.getDriveDistanceInchesLeft() - startDistance) >= distanceTerminal) {
+ autonomousStep += 1;
+ leftpower = 0;
+ rightpower = 0;
+ }
+ break;
+ case 9:
+ if (robot.isTargetFound() && robot.canShoot()){
+ robot.setActivelyShooting(true);
+ startTime = System.currentTimeMillis();
+ autonomousStep += 1.00;
+ }
+ break;
+ case 10:
+ if (System.currentTimeMillis() - startTime >= 500){
+ robot.setActivelyShooting(false);
+ autonomousStep += 1;
+ }
+ case 11:
+ leftpower = 0;
+ rightpower = 0;
+ break;
+ }
+ robot.drivePercent(leftpower, rightpower);
+
+
+
+ }
+}
diff --git a/src/main/java/frc/robot/autonomous/SimpleCTerminal.java b/src/main/java/frc/robot/autonomous/BallCtoB.java
similarity index 53%
rename from src/main/java/frc/robot/autonomous/SimpleCTerminal.java
rename to src/main/java/frc/robot/autonomous/BallCtoB.java
index aacd49a..78b532e 100644
--- a/src/main/java/frc/robot/autonomous/SimpleCTerminal.java
+++ b/src/main/java/frc/robot/autonomous/BallCtoB.java
@@ -1,67 +1,39 @@
package frc.robot.autonomous;
import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.generic.GenericRobot;
//Simple autonomous code for ball C, closest ball to the hangar, and driving to the ball at terminal
-public class SimpleCTerminal extends GenericAutonomous {
+public class BallCtoB extends GenericAutonomous {
double startingYaw;
+
double startDistance;
double leftpower;
double rightpower;
- double defaultPower = .25;
- double defaultTurnPower = .25;
-
+ double defaultPower = .4;
+ double defaultTurnPower = .4;
double correction;
double startTime;
- PIDController PIDDriveStraight;
+ double distanceC = 40.44;
+ double distanceB = 0;
+ double angleC = 84.54;
+ double rampDownDist = 10;
- //
- int averageTurretXSize = 6;
- double[] averageTurretX = new double [averageTurretXSize];
- double turretx;
- double turrety;
- double turretarea;
- double turretv;
- int counter = 0;
- PIDController turretPIDController;
- //
+ PIDController PIDDriveStraight;
@Override
public void autonomousInit(GenericRobot robot) {
autonomousStep = 0;
- startingYaw = robot.getYaw(); //might need to change to set degrees
+ startingYaw = robot.getYaw();
startTime = System.currentTimeMillis();
PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD());
- turretPIDController = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD());
}
@Override
public void autonomousPeriodic(GenericRobot robot) {
- //
- NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
- NetworkTableEntry tx = table.getEntry("tx");
- NetworkTableEntry ty = table.getEntry("ty");
- NetworkTableEntry ta = table.getEntry("ta");
- NetworkTableEntry tv = table.getEntry("tv");
-
- turretx = tx.getDouble(0.0);
- turrety = ty.getDouble(0.0);
- turretarea = ta.getDouble(0.0);
- turretv = tv.getDouble(0.0);
- //
-
-
- SmartDashboard.putNumber("Autonomous Step", autonomousStep);
- SmartDashboard.putNumber("Position", robot.getDriveDistanceInchesLeft());
- SmartDashboard.putNumber("Starting Yaw", startingYaw);
- SmartDashboard.putNumber("Current Yaw", robot.getYaw());
switch(autonomousStep){
case 0: //reset
@@ -85,22 +57,20 @@ public void autonomousPeriodic(GenericRobot robot) {
leftpower = defaultPower + correction;
rightpower = defaultPower - correction;
- if (robot.getDriveDistanceInchesLeft() - startDistance >= 27){
- double a = rampDown(defaultPower, 0, startDistance, 10, robot.getDriveDistanceInchesLeft(), 37);
- leftpower = a;
- rightpower = a;
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceC - rampDownDist){
+ defaultPower = (distanceC-robot.getDriveDistanceInchesLeft()+startDistance)
+ *defaultPower/rampDownDist;
}
-
- if(robot.getDriveDistanceInchesLeft() - startDistance >= 37) {
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceC){
autonomousStep += 1;
- } //has 3 inches of momentum with .25 power
+ startTime = System.currentTimeMillis();
+ }
break;
case 5: //stop
leftpower = 0;
rightpower = 0;
startDistance = robot.getDriveDistanceInchesLeft();
autonomousStep = 12;
- //autonomousStep = 8;
break;
case 6: //collector to collect ball
case 7: //collection part 2 not electric nor boogaloo
@@ -114,8 +84,8 @@ public void autonomousPeriodic(GenericRobot robot) {
rightpower = defaultTurnPower;
//turning left
- if(robot.getYaw() - startingYaw < -84.54) {
- startingYaw = startingYaw - 84.54;
+ if(robot.getYaw() - startingYaw < -angleC) {
+ startingYaw = startingYaw - angleC;
startDistance = robot.getDriveDistanceInchesLeft();
PIDDriveStraight.reset();
autonomousStep += 1;
@@ -127,16 +97,14 @@ public void autonomousPeriodic(GenericRobot robot) {
leftpower = defaultPower + correction;
rightpower = defaultPower - correction;
- if (robot.getDriveDistanceInchesLeft() - startDistance >= 241){
- double a = rampDown(defaultPower, 0, startDistance, 10, robot.getDriveDistanceInchesLeft(), 251);
- leftpower = a;
- rightpower = a;
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceB - rampDownDist){
+ defaultPower = (distanceB -robot.getDriveDistanceInchesLeft()+startDistance)
+ *defaultPower/rampDownDist;
}
- if(robot.getDriveDistanceInchesLeft() - startDistance >= 251) {
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceB){
autonomousStep += 1;
- leftpower = 0;
- rightpower = 0;
- } //might need to tune for momentum
+ startTime = System.currentTimeMillis();
+ }
break;
case 14:
leftpower = 0;
@@ -144,28 +112,5 @@ public void autonomousPeriodic(GenericRobot robot) {
break;
}
robot.drivePercent(leftpower, rightpower);
-
- //If turret works set value of averageTurretX[] to turretx
- if(turretv !=0 ) {
- averageTurretX[counter % averageTurretXSize] = turretx;
- counter++;
- }
-
- double average = 0;
- for(double i: averageTurretX){
- average += i;
- }
- average /= averageTurretXSize;
-
- double currentTurretPower = 0;
-
- if(turretv !=0){
- currentTurretPower = turretPIDController.calculate(average);
- }else{
- turretPIDController.reset();
- }
-
- robot.setTurretPowerPct(currentTurretPower);
-
}
}
diff --git a/src/main/java/frc/robot/autonomous/BallCtoBtoTerminal.java b/src/main/java/frc/robot/autonomous/BallCtoBtoTerminal.java
new file mode 100644
index 0000000..4603dcc
--- /dev/null
+++ b/src/main/java/frc/robot/autonomous/BallCtoBtoTerminal.java
@@ -0,0 +1,151 @@
+package frc.robot.autonomous;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.generic.GenericRobot;
+
+//Simple autonomous code
+public class BallCtoBtoTerminal extends GenericAutonomous{
+ double startingYaw;
+ double startTime;
+ double startDistance;
+
+ double leftpower;
+ double rightpower;
+ double defaultPower = .25;
+ double defaultTurnPower = .25;
+ double correction;
+
+ double distnacetoC = 37;
+ double distancetoB = 169.95;
+ double distancetoTerminal = 160.6;
+ double angleC = 56.25;
+ double angleB = 81.27;
+ double rampDownDist = 10;
+
+ PIDController PIDDriveStraight;
+
+ @Override
+ public void autonomousInit(GenericRobot robot) {
+ autonomousStep = 0;
+ startingYaw = robot.getYaw();
+ startTime = System.currentTimeMillis();
+ PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDpivotD());
+ }
+
+ @Override
+ public void autonomousPeriodic(GenericRobot robot) {
+
+ switch (autonomousStep) {
+ case 0: //reset
+ robot.lowerCollector();
+ PIDDriveStraight.reset();
+ PIDDriveStraight.enableContinuousInput(-180, 180);
+ robot.resetEncoders();
+ if (System.currentTimeMillis() - startTime > 100) {
+ autonomousStep = 4;
+ startingYaw = robot.getYaw();
+ startDistance = robot.getDriveDistanceInchesLeft();
+ }
+ break;
+ case 1: //shoot the ball
+ case 2: //shoot the ball part 2 electric boogaloo
+ case 3: //shoot the ball part 3 maybe
+ case 4: //drive to ball A
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if (robot.getDriveDistanceInchesLeft() - startDistance >= distnacetoC - rampDownDist) {
+ defaultPower = (distnacetoC - robot.getDriveDistanceInchesLeft() + startDistance) * defaultPower / rampDownDist;
+ }
+ if (robot.getDriveDistanceInchesLeft() - startDistance >= distnacetoC) {
+ autonomousStep += 1;
+ startTime = System.currentTimeMillis();
+ }
+ break;
+ case 5: //stop
+ leftpower = 0;
+ rightpower = 0;
+ if (System.currentTimeMillis() - startTime > 1000) {
+ autonomousStep = 12;
+ }
+ break;
+ case 6: //collector to collect ball
+ case 7: //collection part 2 not electric nor boogaloo
+ case 8: //nother collection case
+ case 9: //shoot the second ball for funsies
+ case 10: //miss the target and become sadge
+ case 11: //copium
+ //will change these comments when they actually mean something
+ case 12: //turn to go to ball B
+ leftpower = defaultTurnPower;
+ rightpower = -defaultTurnPower;
+ //turning right
+
+ if (robot.getYaw() - startingYaw > angleC) {
+ startingYaw = startingYaw + angleC;
+ startDistance = robot.getDriveDistanceInchesLeft();
+ PIDDriveStraight.reset();
+ autonomousStep += 1;
+ }
+ break;
+ case 13: //drive towards the ball B
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if (robot.getDriveDistanceInchesLeft() - startDistance >= distancetoB - rampDownDist) {
+ defaultPower = (distancetoB - robot.getDriveDistanceInchesLeft() + startDistance) * defaultPower / rampDownDist;
+ }
+ if (robot.getDriveDistanceInchesLeft() - startDistance >= distancetoB) {
+ autonomousStep += 1;
+ leftpower = 0;
+ rightpower = 0;
+ }
+ break;
+ case 14: //collect / shoot
+ case 15:
+ case 16:
+ case 17:
+ case 18:
+ case 19:
+ case 20:
+ case 21: //turn to ball Terminal
+ leftpower = -defaultTurnPower;
+ rightpower = defaultTurnPower;
+ //turning ???
+
+ if (robot.getYaw() - startingYaw > angleB) {
+ startingYaw = robot.getYaw();
+ startDistance = robot.getDriveDistanceInchesLeft();
+ autonomousStep += 1;
+ }
+ break;
+ case 22: //drive to ball Terminal
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if (robot.getDriveDistanceInchesLeft() - startDistance >= distancetoTerminal - rampDownDist) {
+ defaultPower = (distancetoTerminal - robot.getDriveDistanceInchesLeft() + startDistance) * defaultPower / rampDownDist;
+ }
+ if (robot.getDriveDistanceInchesLeft() - startDistance >= distancetoTerminal) {
+ autonomousStep += 1;
+ leftpower = 0;
+ rightpower = 0;
+ }
+ break;
+ case 23:
+ leftpower = 0;
+ rightpower = 0;
+ break;
+ case 24:
+ case 25:
+ }
+ }
+}
+
diff --git a/src/main/java/frc/robot/autonomous/BallCtoTerminal.java b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java
new file mode 100644
index 0000000..389b07f
--- /dev/null
+++ b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java
@@ -0,0 +1,231 @@
+package frc.robot.autonomous;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import frc.robot.generic.GenericRobot;
+
+//Simple autonomous code for ball C, closest ball to the hangar, and driving to the ball at terminal
+//Setup: Line the robot straight between ball C and the center point of the hub
+public class BallCtoTerminal extends GenericAutonomous {
+ double startingYaw;
+ double startDistance;
+ double startTime;
+
+ double leftpower;
+ double rightpower;
+ double defaultPower = .4;
+ double defaultTurnPower = .4;
+ double correction;
+ boolean time = false;
+
+ double distanceC = 47.9;
+ double distanceTerminal = 251;
+ double angleC = 82.74;
+ double rampDownDist = 10;
+
+ double shooterTargetRPM;
+
+ PIDController PIDDriveStraight;
+ PIDController PIDTurret;
+ PIDController PIDPivot;
+
+ int averageTurretXSize = 2;
+ double[] averageTurretX = new double [averageTurretXSize];
+ double turretx;
+ double turrety;
+ double turretarea;
+ double turretv;
+ int counter = 0;
+
+ boolean readyToShoot = false;
+
+ //TurretTracker tracker = new TurretTracker();
+
+
+ boolean initialTurretSpin = true;
+ @Override
+ public void autonomousInit(GenericRobot robot) {
+ autonomousStep = 0;
+ startingYaw = robot.getYaw();
+ startTime = System.currentTimeMillis();
+ shooterTargetRPM = 0;
+ readyToShoot = false;
+ PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD());
+ PIDPivot = new PIDController(robot.getPIDpivotP(), robot.getPIDpivotI(), robot.getPIDpivotD());
+ PIDTurret = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD());
+
+ //tracker.turretInit(robot);
+
+ }
+
+ @Override
+ public void autonomousPeriodic(GenericRobot robot) {
+ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
+ NetworkTableEntry tx = table.getEntry("tx");
+ NetworkTableEntry ty = table.getEntry("ty");
+ NetworkTableEntry ta = table.getEntry("ta");
+ NetworkTableEntry tv = table.getEntry("tv");
+
+ turretx = tx.getDouble(0.0);
+ turrety = ty.getDouble(0.0);
+ turretarea = ta.getDouble(0.0);
+ turretv = tv.getDouble(0.0);
+ //tracker.turretUpdate(robot);
+
+
+
+
+ if (autonomousStep >= 1){
+ robot.getCargo();
+ robot.shoot();
+ robot.setTurretPitchPosition(.38);
+ }
+ switch(autonomousStep){
+ case 0: //reset
+ robot.lowerCollector();
+ PIDPivot.reset();
+ PIDPivot.enableContinuousInput(-180,180);
+ PIDDriveStraight.reset();
+ PIDDriveStraight.enableContinuousInput(-180,180);
+ robot.resetEncoders();
+ robot.resetAttitude();
+ if (System.currentTimeMillis() - startTime > 100){
+ autonomousStep += 1;
+ startingYaw = robot.getYaw();
+ startDistance = robot.getDriveDistanceInchesLeft();
+ }
+ break;
+ case 1: //create a target shooter value and see if shooter reaches it.
+ if (robot.canShoot()){
+ robot.setActivelyShooting(true);
+ startTime = System.currentTimeMillis();
+ autonomousStep += 1;
+ }
+ break;
+ case 2: //shoot the ball part 2
+ if (System.currentTimeMillis()-startTime >= 250){
+ robot.setActivelyShooting(false);
+ autonomousStep += 1;
+ }
+ break;
+ case 3: //drive to ball C
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceC - rampDownDist){
+ double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist,
+ robot.getDriveDistanceInchesLeft(), distanceC);
+ leftpower = ramp;
+ rightpower = ramp;
+ }
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceC){
+ autonomousStep += 1;
+ startTime = System.currentTimeMillis();
+ }
+ break;
+ case 4: //stop
+ leftpower = 0;
+ rightpower = 0;
+ startDistance = robot.getDriveDistanceInchesLeft();
+ autonomousStep += 1;
+ time = false;
+ break;
+ case 5: //turret turn
+ if (robot.canShoot()){
+ robot.setActivelyShooting(true);
+ startTime = System.currentTimeMillis();
+ autonomousStep += 1.0;
+ }
+ break;
+ case 6:
+ if (System.currentTimeMillis() - startTime >= 500){
+ robot.setActivelyShooting(false);
+ autonomousStep += 1;
+ }
+ case 7://reset
+ PIDDriveStraight.reset();
+ PIDDriveStraight.enableContinuousInput(-180,180);
+ startDistance = robot.getDriveDistanceInchesLeft();
+ autonomousStep +=1;
+ break;
+ case 8: //turn to go to ball @ terminal
+ correction = PIDPivot.calculate(angleC + robot.getYaw() - startingYaw );
+ leftpower = correction;
+ rightpower = -correction;
+ //turning left
+ if (Math.abs(Math.abs(robot.getYaw() - startingYaw)-angleC) <= 1.5){
+ if (!time){
+ startTime = System.currentTimeMillis();
+ time = true;
+ }
+ }
+ else{
+ startTime = System.currentTimeMillis();
+ time = false;
+ }
+ if (System.currentTimeMillis() - startTime >= 50){
+ leftpower = 0;
+ rightpower = 0;
+ autonomousStep += 1;
+ startingYaw = -angleC;
+ startDistance = robot.getDriveDistanceInchesLeft();
+ startTime = System.currentTimeMillis();
+ }
+ break;
+ case 9: //drive towards the ball
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal - rampDownDist){
+ double ramp = rampDown(defaultPower, 0, startDistance, 10,
+ robot.getDriveDistanceInchesLeft(), distanceTerminal);
+ leftpower = ramp;
+ rightpower = ramp;
+ }
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal){
+ autonomousStep += 1;
+ startTime = System.currentTimeMillis();
+ }
+ break;
+ case 10:
+ leftpower = 0;
+ rightpower = 0;
+ break;
+ }
+ robot.drivePercent(leftpower, rightpower);
+ robot.setShooterRPM(shooterTargetRPM, shooterTargetRPM);
+ //If turret works set value of averageTurretX[] to turretx
+
+
+ if(turretv !=0 ) {
+ averageTurretX[counter % averageTurretXSize] = turretx;
+ counter++;
+ }
+
+ double average = 0;
+ for(double i: averageTurretX){
+ average += i;
+ }
+ average /= averageTurretXSize;
+
+ double currentTurretPower = 0;
+
+ if(turretv !=0){
+ currentTurretPower = PIDTurret.calculate(average);
+ }else{
+ PIDTurret.reset();
+ }
+ if((turretv == 0) && (System.currentTimeMillis() - startTime < 2000)) {
+ currentTurretPower = .2;
+ }
+ robot.setTurretPowerPct(currentTurretPower);
+
+ //tracker.turretMove(robot);
+ }
+}
diff --git a/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java
new file mode 100644
index 0000000..5b8ad8f
--- /dev/null
+++ b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java
@@ -0,0 +1,257 @@
+package frc.robot.autonomous;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import frc.robot.generic.GenericRobot;
+
+//Simple autonomous code for ball C, closest ball to the hangar, and driving to the ball at terminal
+//Setup: Line the robot straight between ball C and the center point of the hub
+public class BallCtoTerminalReturn extends GenericAutonomous {
+ double startingYaw;
+ double startDistance;
+ double startTime;
+
+ double leftpower;
+ double rightpower;
+ double defaultPower = .6;
+ double correction;
+ boolean time = false;
+
+ double distanceC = 47.9;
+ double distanceTerminal = 219;
+ double angleC = 84.74;
+ double rampDownDist = 10;
+
+ PIDController PIDDriveStraight;
+ PIDController PIDTurret;
+ PIDController PIDPivot;
+
+ int averageTurretXSize = 2;
+ double[] averageTurretX = new double [averageTurretXSize];
+ int counter = 0;
+
+ double shooterTargetRPM;
+ boolean readyToShoot;
+
+ double indexerPct;
+ double collectorPct;
+
+
+
+ @Override
+ public void autonomousInit(GenericRobot robot) {
+ autonomousStep = 0;
+ startingYaw = robot.getYaw();
+ startTime = System.currentTimeMillis();
+ shooterTargetRPM = 0;
+ indexerPct = 0;
+ collectorPct = 0;
+ readyToShoot = false;
+ PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD());
+ PIDPivot = new PIDController(robot.getPIDpivotP(), robot.getPIDpivotI(), robot.getPIDpivotD());
+ PIDTurret = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD());
+ robot.setPipeline(0);
+ }
+
+ @Override
+ public void autonomousPeriodic(GenericRobot robot) {
+
+ if(robot.isTargetFound()) {
+ averageTurretX[counter % averageTurretXSize] = robot.getTargetX();
+ counter++;
+ }
+
+ double average = 0;
+ for(double i: averageTurretX){
+ average += i;
+ }
+ average /= averageTurretXSize;
+
+ double currentTurretPower = 0;
+
+ if(robot.isTargetFound()){
+ currentTurretPower = PIDTurret.calculate(average);
+ }else{
+ PIDTurret.reset();
+ }
+
+ if (autonomousStep < 4){
+ if((!robot.isTargetFound()) && (System.currentTimeMillis() - startTime < 5000)) {
+ currentTurretPower = .3;
+ }
+ }
+ if ((autonomousStep>=4) && (autonomousStep < 8)){
+ if((!robot.isTargetFound()) && (System.currentTimeMillis() - startTime < 5000)) {
+ currentTurretPower = -.2;
+ }
+ }
+ robot.setTurretPowerPct(currentTurretPower);
+
+ if (autonomousStep >= 1){
+ robot.getCargo();
+ robot.shoot();
+ if (autonomousStep <= 8){
+ robot.setShooterTargetRPM(3700);
+ }
+ else{
+ robot.setShooterTargetRPM(3900);
+ }
+ }
+ if (autonomousStep >= 1 && autonomousStep <=10){
+ robot.setTurretPitchPosition(.38);
+ }
+ else{
+ robot.setCollectorIntakePercentage(0);
+ robot.setTurretPowerPct(0);
+ }
+ switch(autonomousStep){
+ case 0: //reset
+ robot.lowerCollector();
+ PIDPivot.reset();
+ PIDPivot.enableContinuousInput(-180,180);
+ PIDDriveStraight.reset();
+ PIDDriveStraight.enableContinuousInput(-180,180);
+ robot.resetEncoders();
+ robot.resetAttitude();
+ if (System.currentTimeMillis() - startTime > 500){
+ autonomousStep += 1;
+ startingYaw = robot.getYaw();
+ startDistance = robot.getDriveDistanceInchesLeft();
+ }
+ break;
+ case 1: //drive to ball C
+ collectorPct = 1;
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceC - rampDownDist){
+ double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist,
+ robot.getDriveDistanceInchesLeft(), distanceC);
+ leftpower = ramp;
+ rightpower = ramp;
+ }
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceC){
+ autonomousStep += 1;
+ startTime = System.currentTimeMillis();
+ }
+ break;
+ case 2: //stop
+ leftpower = 0;
+ rightpower = 0;
+ startDistance = robot.getDriveDistanceInchesLeft();
+ autonomousStep += 1;
+ time = false;
+ break;
+ case 3: //create a target shooter value and see if shooter reaches it.
+ if (robot.isTargetFound() && robot.canShoot() && (-5 < average) && (average< 5)){
+ robot.setActivelyShooting(true);
+ startTime = System.currentTimeMillis();
+ autonomousStep += 1;
+ }
+ break;
+ case 4: //turn the shooter off
+ if (System.currentTimeMillis() - startTime >= 2000){
+ robot.setActivelyShooting(false);
+ autonomousStep += 1;
+ }
+ break;
+
+ case 5://reset
+ PIDDriveStraight.reset();
+ PIDDriveStraight.enableContinuousInput(-180,180);
+ startDistance = robot.getDriveDistanceInchesLeft();
+ autonomousStep +=1;
+ break;
+ case 6: //turn to go to Ball Terminal
+ correction = PIDPivot.calculate(angleC + robot.getYaw() - startingYaw );
+ leftpower = correction;
+ rightpower = -correction;
+ robot.setPipeline(1);
+ //turning left
+ if (Math.abs(Math.abs(robot.getYaw() - startingYaw)-angleC) <= 1.5){
+ if (!time){
+ startTime = System.currentTimeMillis();
+ time = true;
+ }
+ }
+ else{
+ startTime = System.currentTimeMillis();
+ time = false;
+ }
+ if (System.currentTimeMillis() - startTime >= 50){
+ leftpower = 0;
+ rightpower = 0;
+ autonomousStep += 1;
+ startingYaw = -angleC;
+ startDistance = robot.getDriveDistanceInchesLeft();
+ startTime = System.currentTimeMillis();
+ }
+ break;
+ case 7: //drive towards Ball Terminal
+
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal - rampDownDist){
+ double ramp = rampDown(defaultPower, .1, startDistance, 10,
+ robot.getDriveDistanceInchesLeft(), distanceTerminal);
+ leftpower = ramp;
+ rightpower = ramp;
+ }
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal){
+ autonomousStep += 1;
+ startDistance = robot.getDriveDistanceInchesLeft();
+ startTime = System.currentTimeMillis();
+ }
+ break;
+ case 8: //collect Ball Terminal
+ leftpower = 0;
+ rightpower = 0;
+ autonomousStep += 1;
+ robot.setPipeline(0);
+ break;
+ case 9: //Drive back to Ball C
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = -1*(defaultPower - correction);
+ rightpower = -1*(defaultPower + correction);
+
+ if(Math.abs(robot.getDriveDistanceInchesLeft() - startDistance) >= distanceTerminal - rampDownDist){
+ double ramp = rampDown(defaultPower, 0, startDistance, 10,
+ robot.getDriveDistanceInchesLeft(), distanceTerminal);
+ leftpower = -ramp;
+ rightpower = -ramp;
+ }
+ if(Math.abs(robot.getDriveDistanceInchesLeft() - startDistance) >= distanceTerminal){
+ leftpower = 0;
+ rightpower = 0;
+ autonomousStep += 1;
+ }
+ break;
+ case 10: //shoot it :)
+ if (robot.canShoot()){
+ robot.setActivelyShooting(true);
+ startTime = System.currentTimeMillis();
+ autonomousStep += 1.00;
+ }
+ break;
+ case 11: //shoot part 2
+ if (System.currentTimeMillis() - startTime >= 1000){
+ robot.setActivelyShooting(false);
+ autonomousStep += 1.0;
+ }
+ break;
+ case 12: //End of autonomous, wait for Teleop
+ leftpower = 0;
+ rightpower = 0;
+ break;
+ }
+ robot.drivePercent(leftpower, rightpower);
+ }
+}
diff --git a/src/main/java/frc/robot/autonomous/BallSimpleB.java b/src/main/java/frc/robot/autonomous/BallSimpleB.java
new file mode 100644
index 0000000..aba6cee
--- /dev/null
+++ b/src/main/java/frc/robot/autonomous/BallSimpleB.java
@@ -0,0 +1,76 @@
+package frc.robot.autonomous;
+
+import edu.wpi.first.math.controller.PIDController;
+import frc.robot.generic.GenericRobot;
+
+//Setup: put the robot down so that the ball at the terminal and the ball B are lined up straight.
+public class BallSimpleB extends GenericAutonomous {
+ double startingYaw;
+
+ double startDistance;
+ double leftpower;
+ double rightpower;
+ double defaultPower = .4;
+ double correction;
+ double startTime;
+
+ double distanceB = 61.5; //distance to ball B from the tarmac
+ double rampDownDist = 10;
+
+ PIDController PIDDriveStraight;
+
+ @Override
+ public void autonomousInit(GenericRobot robot){
+ autonomousStep = 0;
+ startingYaw = robot.getYaw();
+ startTime = System.currentTimeMillis();
+ PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD());
+ }
+
+ @Override
+ public void autonomousPeriodic(GenericRobot robot){
+
+ switch(autonomousStep){
+ case 0: //reset
+ robot.lowerCollector();
+ PIDDriveStraight.reset();
+ PIDDriveStraight.enableContinuousInput(-180,180);
+ robot.resetEncoders();
+ robot.resetAttitude();
+ if (System.currentTimeMillis() - startTime > 100){
+ autonomousStep = 4;
+ startingYaw = robot.getYaw();
+ startDistance = robot.getDriveDistanceInchesLeft();
+ }
+ break;
+ case 1: //shoot the ball
+ case 2: //shoot the ball part 2 electric boogaloo
+ case 3: //shoot the ball part 3 maybe
+ case 4: //drive to ball A
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceB - rampDownDist){
+ defaultPower = (distanceB - robot.getDriveDistanceInchesLeft() + startDistance)
+ * defaultPower / rampDownDist;
+ }
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceB){
+ autonomousStep += 1;
+ startTime = System.currentTimeMillis();
+ }
+ break;
+ case 5: //stop
+ leftpower = 0;
+ rightpower = 0;
+ startDistance = robot.getDriveDistanceInchesLeft();
+ autonomousStep = 12;
+ break;
+ case 6: //collect/shoot the ball again
+ }
+ robot.drivePercent(leftpower, rightpower);
+ }
+
+
+}
diff --git a/src/main/java/frc/robot/autonomous/BallSimpleC.java b/src/main/java/frc/robot/autonomous/BallSimpleC.java
new file mode 100644
index 0000000..fc2ff8a
--- /dev/null
+++ b/src/main/java/frc/robot/autonomous/BallSimpleC.java
@@ -0,0 +1,103 @@
+package frc.robot.autonomous;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.generic.GenericRobot;
+
+//Simple autonomous code for ball C
+//Setup: Line the robot straight between ball C and the center point of the hub
+public class BallSimpleC extends GenericAutonomous {
+ double startingYaw;
+
+ double startDistance;
+ double leftpower;
+ double rightpower;
+ double defaultPower = .4;
+ double correction;
+ double startTime;
+
+ double distanceC = 47.9;
+ double distance = 10;
+ double rampDownDist = 10;
+
+ PIDController PIDDriveStraight;
+
+ @Override
+ public void autonomousInit(GenericRobot robot){
+ autonomousStep = 0;
+ startingYaw = robot.getYaw();
+ startTime = System.currentTimeMillis();
+ PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD());
+ }
+
+ @Override
+ public void autonomousPeriodic(GenericRobot robot){
+
+ switch(autonomousStep){
+ case 0: //reset
+ robot.lowerCollector();
+ PIDDriveStraight.reset();
+ PIDDriveStraight.enableContinuousInput(-180,180);
+ robot.resetEncoders();
+ robot.resetAttitude();
+ if (System.currentTimeMillis() - startTime > 100){
+ autonomousStep = 4;
+ startingYaw = robot.getYaw();
+ startDistance = robot.getDriveDistanceInchesLeft();
+ }
+ break;
+ case 1: //shoot the ball
+ case 2: //shoot the ball part 2 electric boogaloo
+ case 3: //shoot the ball part 3 maybe
+ case 4: //drive to ball A
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceC - rampDownDist){
+ double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist,
+ robot.getDriveDistanceInchesLeft(), distanceC);
+ leftpower = ramp;
+ rightpower = ramp;
+ }
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceC){
+ autonomousStep += 1;
+ startTime = System.currentTimeMillis();
+ }
+ break;
+ case 5: //stop
+ leftpower = 0;
+ rightpower = 0;
+ startDistance = robot.getDriveDistanceInchesLeft();
+ autonomousStep = 12;
+ break;
+ case 6: //collect/shoot the ball again
+ case 7:
+ case 8:
+ case 9:
+ case 10:
+ case 11: //moving forward 10 inches to completely clear the tarmac
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distance - rampDownDist){
+ double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist,
+ robot.getDriveDistanceInchesLeft(), distance);
+ leftpower = ramp;
+ rightpower = ramp;
+ }
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distance){
+ autonomousStep += 1;
+ }
+ break;
+ case 12: //stop
+ leftpower = 0;
+ rightpower = 0;
+ break;
+ }
+ robot.drivePercent(leftpower, rightpower);
+ }
+}
diff --git a/src/main/java/frc/robot/autonomous/Calibration.java b/src/main/java/frc/robot/autonomous/Calibration.java
new file mode 100644
index 0000000..ecdaac1
--- /dev/null
+++ b/src/main/java/frc/robot/autonomous/Calibration.java
@@ -0,0 +1,71 @@
+package frc.robot.autonomous;
+
+import edu.wpi.first.math.controller.PIDController;
+import frc.robot.generic.GenericRobot;
+
+//calibrating tool for encoder ratio of our bots, inches/ticks
+public class Calibration extends GenericAutonomous {
+ double startingYaw;
+
+ double startDistance;
+ double leftpower;
+ double rightpower;
+ double defaultPower = .3;
+ double correction;
+ double startTime;
+
+ double distance = 72;
+ double rampDownDist = 10;
+
+ PIDController PIDDriveStraight;
+
+ @Override
+ public void autonomousInit(GenericRobot robot){
+ autonomousStep = 0;
+ startingYaw = robot.getYaw();
+ startTime = System.currentTimeMillis();
+ PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD());
+ }
+
+ @Override
+ public void autonomousPeriodic(GenericRobot robot){
+
+ switch(autonomousStep){
+ case 0: //reset
+ robot.lowerCollector();
+ PIDDriveStraight.reset();
+ PIDDriveStraight.enableContinuousInput(-180,180);
+ robot.resetEncoders();
+ robot.resetAttitude();
+ if (System.currentTimeMillis() - startTime > 100){
+ autonomousStep += 1;
+ startingYaw = robot.getYaw();
+ startDistance = robot.getDriveDistanceInchesLeft();
+ }
+ break;
+ case 1: //drive forward 6ft
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distance - rampDownDist){
+ double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist,
+ robot.getDriveDistanceInchesLeft(), distance);
+ leftpower = ramp;
+ rightpower = ramp;
+ }
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distance){
+ autonomousStep += 1;
+ startTime = System.currentTimeMillis();
+ }
+ break;
+ case 2: //stop
+ leftpower = 0;
+ rightpower = 0;
+ break;
+ }
+ robot.drivePercent(leftpower, rightpower);
+ }
+}
diff --git a/src/main/java/frc/robot/autonomous/GenericAutonomous.java b/src/main/java/frc/robot/autonomous/GenericAutonomous.java
index 81a61c2..c98521a 100644
--- a/src/main/java/frc/robot/autonomous/GenericAutonomous.java
+++ b/src/main/java/frc/robot/autonomous/GenericAutonomous.java
@@ -7,7 +7,7 @@
public abstract class GenericAutonomous {
- public int autonomousStep;
+ public int autonomousStep = 0;
public void autonomousInit(GenericRobot robot) {
//System.out.println("I don't have autonomousInit in my autonomous program :'(");
@@ -19,7 +19,7 @@ public void autonomousPeriodic(GenericRobot robot) {
public double rampDown(double startPower, double endPower, double startDistance, double rolloutDistance, double currentDist, double endDist){
- double power = (endDist-currentDist+startDistance)*(startPower-endPower)/rolloutDistance+endPower;
+ double power = (endDist-Math.abs(currentDist-startDistance))*(startPower-endPower)/rolloutDistance+endPower;
return power;
}
}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/autonomous/ShortRun.java b/src/main/java/frc/robot/autonomous/ShortRun.java
new file mode 100644
index 0000000..56e9b38
--- /dev/null
+++ b/src/main/java/frc/robot/autonomous/ShortRun.java
@@ -0,0 +1,164 @@
+package frc.robot.autonomous;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import frc.robot.generic.GenericRobot;
+
+//Simple autonomous code for ball C, closest ball to the hangar, and driving to the ball at terminal
+//Setup: Line the robot straight between ball C and the center point of the hub
+public class ShortRun extends GenericAutonomous {
+ double startingYaw;
+ double startDistance;
+ double startTime;
+
+ double leftpower;
+ double rightpower;
+ double defaultPower = .6;
+ double correction;
+ boolean time = false;
+
+ double distanceC = 47.9;
+ double distanceTerminal = 225;
+ double angleC = 82.74;
+ double rampDownDist = 10;
+
+ PIDController PIDDriveStraight;
+ PIDController PIDTurret;
+ PIDController PIDPivot;
+
+ int averageTurretXSize = 2;
+ double[] averageTurretX = new double [averageTurretXSize];
+ int counter = 0;
+
+ double shooterTargetRPM;
+ boolean readyToShoot;
+
+ double indexerPct;
+ double collectorPct;
+
+
+
+ @Override
+ public void autonomousInit(GenericRobot robot) {
+ autonomousStep = 0;
+ startingYaw = robot.getYaw();
+ startTime = System.currentTimeMillis();
+ shooterTargetRPM = 0;
+ indexerPct = 0;
+ collectorPct = 0;
+ readyToShoot = false;
+ PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD());
+ PIDPivot = new PIDController(robot.getPIDpivotP(), robot.getPIDpivotI(), robot.getPIDpivotD());
+ PIDTurret = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD());
+ robot.setPipeline(0);
+ }
+
+ @Override
+ public void autonomousPeriodic(GenericRobot robot) {
+
+ if(robot.isTargetFound()) {
+ averageTurretX[counter % averageTurretXSize] = robot.getTargetX();
+ counter++;
+ }
+
+ double average = 0;
+ for(double i: averageTurretX){
+ average += i;
+ }
+ average /= averageTurretXSize;
+
+ double currentTurretPower = 0;
+
+ if(robot.isTargetFound()){
+ currentTurretPower = PIDTurret.calculate(average);
+ }else{
+ PIDTurret.reset();
+ }
+
+ if (autonomousStep < 4){
+ if((!robot.isTargetFound()) && (System.currentTimeMillis() - startTime < 5000)) {
+ currentTurretPower = .3;
+ }
+ }
+ if ((autonomousStep>=4) && (autonomousStep < 8)){
+ if((!robot.isTargetFound()) && (System.currentTimeMillis() - startTime < 5000)) {
+ currentTurretPower = -.2;
+ }
+ }
+ robot.setTurretPowerPct(currentTurretPower);
+
+ if (autonomousStep >= 1){
+ robot.getCargo();
+ robot.shoot();
+ }
+ if (autonomousStep >= 1 && autonomousStep <=10){
+ robot.setTurretPitchPosition(.38);
+ }
+ else{
+ robot.setCollectorIntakePercentage(0);
+ robot.setTurretPowerPct(0);
+ }
+ switch(autonomousStep){
+ case 0: //reset
+ robot.lowerCollector();
+ PIDPivot.reset();
+ PIDPivot.enableContinuousInput(-180,180);
+ PIDDriveStraight.reset();
+ PIDDriveStraight.enableContinuousInput(-180,180);
+ robot.resetEncoders();
+ robot.resetAttitude();
+ if (System.currentTimeMillis() - startTime > 500){
+ autonomousStep += 1;
+ startingYaw = robot.getYaw();
+ startDistance = robot.getDriveDistanceInchesLeft();
+ }
+ break;
+ case 1: //drive to ball C
+ collectorPct = 1;
+ correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+ leftpower = defaultPower + correction;
+ rightpower = defaultPower - correction;
+
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceC - rampDownDist){
+ double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist,
+ robot.getDriveDistanceInchesLeft(), distanceC);
+ leftpower = ramp;
+ rightpower = ramp;
+ }
+ if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceC){
+ autonomousStep += 1;
+ startTime = System.currentTimeMillis();
+ }
+ break;
+ case 2: //stop
+ leftpower = 0;
+ rightpower = 0;
+ startDistance = robot.getDriveDistanceInchesLeft();
+ autonomousStep += 1;
+ time = false;
+ break;
+ case 3: //create a target shooter value and see if shooter reaches it.
+ if (robot.isTargetFound() && robot.canShoot() && (-5 < average) && (average< 5)){
+ robot.setActivelyShooting(true);
+ startTime = System.currentTimeMillis();
+ autonomousStep += 1;
+ }
+ break;
+ case 4: //turn the shooter off
+ if (System.currentTimeMillis() - startTime >= 2000){
+ robot.setActivelyShooting(false);
+ autonomousStep += 1;
+ }
+ break;
+
+ case 5: //End of autonomous, wait for Teleop
+ leftpower = 0;
+ rightpower = 0;
+ break;
+ }
+ robot.drivePercent(leftpower, rightpower);
+ }
+}
diff --git a/src/main/java/frc/robot/autonomous/TurretTracker.java b/src/main/java/frc/robot/autonomous/TurretTracker.java
new file mode 100644
index 0000000..904cea4
--- /dev/null
+++ b/src/main/java/frc/robot/autonomous/TurretTracker.java
@@ -0,0 +1,59 @@
+package frc.robot.autonomous;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import frc.robot.generic.GenericRobot;
+
+public class TurretTracker {
+ int averageTurretXSize = 2;
+ double[] averageTurretX = new double [averageTurretXSize];
+ double turretx;
+ double turrety;
+ double turretarea;
+ double turretv;
+ int counter = 0;
+ PIDController turretPIDController;
+
+ public void turretInit(GenericRobot robot) {
+ turretPIDController = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD());
+ }
+
+ public void turretUpdate(GenericRobot robot) {
+ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
+ NetworkTableEntry tx = table.getEntry("tx");
+ NetworkTableEntry ty = table.getEntry("ty");
+ NetworkTableEntry ta = table.getEntry("ta");
+ NetworkTableEntry tv = table.getEntry("tv");
+
+ turretx = tx.getDouble(0.0);
+ turrety = ty.getDouble(0.0);
+ turretarea = ta.getDouble(0.0);
+ turretv = tv.getDouble(0.0);
+ }
+
+ public void turretMove(GenericRobot robot) {
+ //If turret works set value of averageTurretX[] to turretx
+ if(turretv !=0 ) {
+ averageTurretX[counter % averageTurretXSize] = turretx;
+ counter++;
+ }
+
+ double average = 0;
+ for(double i: averageTurretX){
+ average += i;
+ }
+ average /= averageTurretXSize;
+
+ double currentTurretPower = 0;
+
+ if(turretv !=0){
+ currentTurretPower = turretPIDController.calculate(average);
+ }else{
+ turretPIDController.reset();
+ }
+
+ robot.setTurretPowerPct(currentTurretPower);
+ }
+}
diff --git a/src/main/java/frc/robot/autonomous/autoArc.java b/src/main/java/frc/robot/autonomous/autoArc.java
index 01f305e..8015d6a 100644
--- a/src/main/java/frc/robot/autonomous/autoArc.java
+++ b/src/main/java/frc/robot/autonomous/autoArc.java
@@ -1,10 +1,10 @@
package frc.robot.autonomous;
-
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.generic.GenericRobot;
+//insert blurb
public class autoArc extends GenericAutonomous {
double rollout = 72; //center of rotation changes per robot. Test value per robot
double radius = 153;
@@ -32,9 +32,10 @@ public class autoArc extends GenericAutonomous {
boolean time = false;
int counter;
double[] averageX = new double [2];
- double currentTurretPower = 0;
- double average;
int averageTurretXSize = 2;
+ boolean breakRobot = false;
+ boolean startTimer = false;
+ boolean collect = false;
@Override
public void autonomousInit(GenericRobot robot) {
@@ -58,34 +59,58 @@ public void autonomousInit(GenericRobot robot) {
autonomousStep = 0;
time = false;
counter = 0;
+ collect = false;
+ robot.setPipeline(0);
}
@Override
public void autonomousPeriodic(GenericRobot robot) {
- // Turret Auto Track
if(robot.isTargetFound()) {
- //take a look at averaging function
- //when we lose sight of target and then see it again.
- counter = counter%averageTurretXSize;
- averageX[counter] = robot.getTargetX();
+ averageX[counter % averageTurretXSize] = robot.getTargetX();
counter++;
}
- average = 0;
+
+ double average = 0;
for(double i: averageX){
average += i;
}
average /= averageTurretXSize;
- if (robot.isTargetFound()){
+ double currentTurretPower = 0;
+
+ if(robot.isTargetFound()){
currentTurretPower = turretPIDController.calculate(average);
}else{
turretPIDController.reset();
- currentTurretPower = 0;
}
- // Turret AutoTrack
+ if((!robot.isTargetFound()) && (System.currentTimeMillis() - startingTime < 5000)) {
+ currentTurretPower = .3;
+ }
+ if ((autonomousStep>=4) && (autonomousStep < 8)){
+ if((!robot.isTargetFound()) && (System.currentTimeMillis() - startingTime < 5000)) {
+ currentTurretPower = -.2;
+ }
+ }
+
+ robot.setTurretPowerPct(currentTurretPower);
+
+
+ if (autonomousStep >= 1){
+ if (autonomousStep <= 4){
+ robot.getCargo();
+ }
+ else if (!collect){
+ robot.setCollectorIntakePercentage(-1);
+ }
+ else{
+ robot.getCargo();
+ }
+ robot.shoot();
+ robot.setTurretPitchPosition(.38);
+ }
switch (autonomousStep){
case 0: //reset
robot.lowerCollector();
@@ -101,7 +126,6 @@ public void autonomousPeriodic(GenericRobot robot) {
autonomousStep += 1;
}
break;
-
case 1: //straightaway
currentYaw = robot.getYaw();
currentDistInches = robot.getDriveDistanceInchesLeft();
@@ -109,25 +133,40 @@ public void autonomousPeriodic(GenericRobot robot) {
leftPower = defaultPower + correction;
rightPower = defaultPower - correction;
if (currentDistInches - startInches >= rollout-rampDownDist){ //slowdown
- double a = rampDown(defaultPower, 0, startInches, rampDownDist, currentDistInches, rollout);
+ double a = rampDown(defaultPower, 0.1, startInches, rampDownDist, currentDistInches, rollout);
leftPower = a;
rightPower = a;
}
if (currentDistInches - startInches >= rollout){
- defaultPower = .4;
leftPower = 0;
rightPower = 0;
autonomousStep += 1;
}
break;
-
- case 2: // Pid pivot 90 degrees ccw
+ case 2:
+ if (robot.canShoot() && robot.isTargetFound() &&(-5= 1000){
+ robot.setActivelyShooting(false);
+ autonomousStep += 1;
+ }
+ break;
+ case 4: // Pid pivot 90 degrees ccw
currentYaw = robot.getYaw();
correction = PIDPivot.calculate(pivotDeg + currentYaw - startYaw);
leftPower = correction;
- rightPower = -correction;
- currentTurretPower = .05;
- if (Math.abs(Math.abs(currentYaw - startYaw)-pivotDeg) <= 1.5){
+ rightPower = -correction; //TODO: on lightning, change to abs encoder
+ if (!robot.isTargetFound() && !breakRobot) {
+ startingTime = System.currentTimeMillis();
+ breakRobot = true;
+ }
+
+ if (Math.abs(Math.abs(currentYaw - startYaw)-pivotDeg) <= 2){
if (!time){
startingTime = System.currentTimeMillis();
time = true;
@@ -145,7 +184,7 @@ public void autonomousPeriodic(GenericRobot robot) {
}
break;
- case 3: //Pid reset
+ case 5: //Pid reset
PIDSteering.reset();
PIDPivot.reset();
PIDSteering.disableContinuousInput();
@@ -155,7 +194,7 @@ public void autonomousPeriodic(GenericRobot robot) {
autonomousStep += 1;
break;
- case 4: //Pid Arc 10 ft Left
+ case 6: //Pid Arc 10 ft Left
currentYaw = robot.getYaw();
currentDistInches = robot.getDriveDistanceInchesRight();
correction = PIDSteering.calculate(Math.toDegrees((currentDistInches-startInches)/outerRadius) + (currentYaw-startYaw));
@@ -167,16 +206,27 @@ public void autonomousPeriodic(GenericRobot robot) {
rightPower = 0;
autonomousStep += 1;
}
+ if (currentDistInches - startInches >= outerArcDist - 24){
+ collect = true;
+ }
break;
- case 5: //stop
+ case 7: //stop
leftPower = 0;
rightPower = 0;
+ autonomousStep += 1;
+ break;
+ case 8: //shoot last cargo
+ if (robot.canShoot()){
+ robot.setActivelyShooting(true);
+ }
break;
+
}
robot.drivePercent(leftPower, rightPower);
- robot.setTurretPowerPct(currentTurretPower);
+
+
}
diff --git a/src/main/java/frc/robot/autonomous/autoArcDifferent.java b/src/main/java/frc/robot/autonomous/autoArcDifferent.java
new file mode 100644
index 0000000..320fcc0
--- /dev/null
+++ b/src/main/java/frc/robot/autonomous/autoArcDifferent.java
@@ -0,0 +1,183 @@
+package frc.robot.autonomous;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.generic.GenericRobot;
+
+//insert blurb
+public class autoArcDifferent extends GenericAutonomous {
+ double rollout = 72; //center of rotation changes per robot. Test value per robot
+ double radius = 153;
+ double wheelBase = 28;
+ double innerRadius = radius - wheelBase/2;
+ double outerRadius = radius + wheelBase/2;
+ double outerArcDist = 180;
+ double defaultPower = .4;
+ double pivotDeg = -90;
+ double rampDownDist = 10;
+
+ double startYaw;
+ double currentYaw;
+ double startInches;
+ double currentDistInches;
+ double startingTime;
+ double leftPower;
+ double rightPower;
+
+ double correction;
+ PIDController PIDSteering;
+ PIDController PIDPivot;
+ PIDController turretPIDController;
+
+ boolean time = false;
+ int counter;
+ double[] averageX = new double [2];
+ double currentTurretPower = 0;
+ double average;
+ int averageTurretXSize = 2;
+
+ @Override
+ public void autonomousInit(GenericRobot robot) {
+ startingTime = System.currentTimeMillis();
+ PIDSteering = new PIDController(
+ robot.getPIDmaneuverP(),
+ robot.getPIDmaneuverI(),
+ robot.getPIDmaneuverD()
+ );
+ PIDPivot = new PIDController(
+ robot.getPIDpivotP(),
+ robot.getPIDpivotI(),
+ robot.getPIDpivotD()
+ );
+ turretPIDController = new PIDController(
+ robot.turretPIDgetP(),
+ robot.turretPIDgetI(),
+ robot.turretPIDgetD()
+ );
+ defaultPower = .4;
+ autonomousStep = 0;
+ time = false;
+ counter = 0;
+ }
+
+ @Override
+ public void autonomousPeriodic(GenericRobot robot) {
+ // Turret Auto Track
+
+ if(robot.isTargetFound()) {
+ //take a look at averaging function
+ //when we lose sight of target and then see it again.
+ counter = counter%averageTurretXSize;
+ averageX[counter] = robot.getTargetX();
+ counter++;
+ }
+ average = 0;
+ for(double i: averageX){
+ average += i;
+ }
+ average /= averageTurretXSize;
+
+ if (robot.isTargetFound()){
+ currentTurretPower = turretPIDController.calculate(average);
+ }else{
+ turretPIDController.reset();
+ currentTurretPower = 0;
+ }
+
+ // Turret AutoTrack
+
+ switch (autonomousStep){
+ case 0: //reset
+ robot.lowerCollector();
+ PIDSteering.reset();
+ PIDPivot.reset();
+ PIDSteering.enableContinuousInput(-180,180);
+ PIDPivot.enableContinuousInput(-180,180);
+ robot.resetAttitude();
+ robot.resetEncoders();
+ startYaw = robot.getYaw();
+ startInches = robot.getDriveDistanceInchesLeft();
+ if (System.currentTimeMillis() - startingTime >= 100){
+ autonomousStep += 1;
+ }
+ break;
+
+ case 1: //straightaway
+ currentYaw = robot.getYaw();
+ currentDistInches = robot.getDriveDistanceInchesLeft();
+ correction = PIDSteering.calculate(currentYaw - startYaw);
+ leftPower = defaultPower + correction;
+ rightPower = defaultPower - correction;
+ if (currentDistInches - startInches >= rollout-rampDownDist){ //slowdown
+ defaultPower = (rollout-currentDistInches+startInches)*defaultPower/rampDownDist;
+ }
+ if (currentDistInches - startInches >= rollout){
+ defaultPower = .4;
+ leftPower = 0;
+ rightPower = 0;
+ autonomousStep += 1;
+ }
+ break;
+
+ case 2: // Pid pivot 90 degrees ccw
+ currentYaw = robot.getYaw();
+ correction = PIDPivot.calculate(pivotDeg + currentYaw - startYaw);
+ leftPower = correction;
+ rightPower = -correction;
+ currentTurretPower = .05;
+ if (Math.abs(Math.abs(currentYaw - startYaw)-pivotDeg) <= 1.5){
+ if (!time){
+ startingTime = System.currentTimeMillis();
+ time = true;
+ }
+ }
+ else{
+ startingTime = System.currentTimeMillis();
+ time = false;
+ }
+ if (System.currentTimeMillis() - startingTime >= 50){
+ leftPower = 0;
+ rightPower = 0;
+ autonomousStep += 1;
+ startingTime = System.currentTimeMillis();
+ }
+ break;
+
+ case 3: //Pid reset
+ PIDSteering.reset();
+ PIDPivot.reset();
+ PIDSteering.disableContinuousInput();
+ PIDPivot.enableContinuousInput(-180, 180);
+ startYaw = startYaw - pivotDeg;
+ startInches = robot.getDriveDistanceInchesLeft();
+ autonomousStep += 1;
+ break;
+
+ case 4: //Pid Arc 10 ft Right
+ currentYaw = robot.getYaw();
+ currentDistInches = robot.getDriveDistanceInchesLeft();
+ correction = PIDSteering.calculate(Math.toDegrees((currentDistInches-startInches)/outerRadius) + (currentYaw-startYaw));
+ double radiusRatio = outerRadius/innerRadius;
+ leftPower = (Math.sqrt(radiusRatio))*defaultPower - correction;
+ rightPower = 1/Math.sqrt(radiusRatio)*defaultPower + correction;
+ if (currentDistInches - startInches >= outerArcDist){
+ leftPower = 0;
+ rightPower = 0;
+ autonomousStep += 1;
+ }
+ break;
+
+ case 5: //stop
+ leftPower = 0;
+ rightPower = 0;
+ break;
+
+ }
+ robot.drivePercent(leftPower, rightPower);
+ robot.setTurretPowerPct(currentTurretPower);
+
+
+
+ }
+
+}
diff --git a/src/main/java/frc/robot/command/GenericCommand.java b/src/main/java/frc/robot/command/GenericCommand.java
index b7ca72d..5b87ecd 100644
--- a/src/main/java/frc/robot/command/GenericCommand.java
+++ b/src/main/java/frc/robot/command/GenericCommand.java
@@ -4,6 +4,7 @@
import frc.robot.generic.GenericRobot;
public abstract class GenericCommand {
+ public int commandStep = -1;
public void begin(GenericRobot robot) {
System.out.println("I don't define begin() steps in my command :'(");
@@ -17,5 +18,10 @@ public void step(GenericRobot robot) {
public boolean enable;
+ public double rampDown(double startPower, double endPower, double startDistance, double rolloutDistance, double currentDist, double endDist){
+ double power = (endDist-Math.abs(currentDist-startDistance))*(startPower-endPower)/rolloutDistance+endPower;
+ return power;
+ }
+
}
diff --git a/src/main/java/frc/robot/command/Hang.java b/src/main/java/frc/robot/command/Hang.java
new file mode 100644
index 0000000..4b79155
--- /dev/null
+++ b/src/main/java/frc/robot/command/Hang.java
@@ -0,0 +1,491 @@
+package frc.robot.command;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.generic.GenericRobot;
+
+public class Hang extends GenericCommand{
+
+
+ double startAngle;
+
+ double leftPower;
+ double rightPower;
+ double defaultPower = .4;
+
+ double startDistance;
+ double differenceDistance;
+
+ double sensorDist = 21.0;
+ double Tapetheta = 0;
+
+ double correction;
+ double currentYaw;
+
+ long startingTime = 0;
+
+ boolean leftSensor = false;
+ boolean rightSensor = false;
+
+ double lTraveled;
+
+ double fwd = 49.5;
+ PIDController PIDSteering;
+ boolean tapeAlign;
+
+ /////^^^^^^^^^^^Stuff for tapeAlign/
+
+
+ //////////////Now the real stuff
+ double escapeHeight = 10;///TODO:what is this??
+ boolean firstTime = true;
+ int countLeft = 0;
+ int countRight = 0;
+ double leftArmPower = 0;
+ double rightArmPower = 0;
+ double defaultClimbPowerUp = .75;
+ double defaultClimbPowerDown = -.75;
+ boolean leftArrived = false;
+ boolean rightArrived = false;
+ double startHeightLeft = 0;
+ double startHeightRight = 0;
+ double turretPower = 0;
+ double level = 7;
+ double leveltol = 2;
+
+ PIDController turretPIDController;
+
+
+ public void begin(GenericRobot robot){
+ startingTime = System.currentTimeMillis();
+ commandStep = -1;
+ leftSensor = false;
+ rightSensor = false;
+ leftArmPower = 0;
+ rightArmPower = 0;
+ leftPower = 0;
+ rightPower = 0;
+ lTraveled = 0;
+ fwd = 49.5;
+ PIDSteering = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD());
+ turretPIDController = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD());
+ tapeAlign = true;
+ firstTime = true;
+ }
+
+ public void step(GenericRobot robot){
+ SmartDashboard.putNumber("tapetheta", Tapetheta);
+ SmartDashboard.putNumber("ltraveled", lTraveled);
+ SmartDashboard.putNumber("fwd", fwd);
+ SmartDashboard.putNumber("leftEncoderRaw", robot.encoderTicksLeftDriveA());
+ SmartDashboard.putNumber("rightEncoderRaw", robot.encoderTicksRightDriveA());
+ SmartDashboard.putBoolean("leftTapeSensor", robot.getFloorSensorLeft());
+ SmartDashboard.putBoolean("rightTapeSensor", robot.getFloorSensorRight());
+ SmartDashboard.putBoolean("leftCLimberSensor", robot.getClimbSensorLeft());
+ SmartDashboard.putBoolean("rightClimberSensor", robot.getClimbSensorRight());
+ SmartDashboard.putNumber("countLeft", countLeft);
+ SmartDashboard.putNumber("countRight", countRight);
+ SmartDashboard.putNumber("startDistance", startDistance);
+
+
+ if (tapeAlign) {
+ if (commandStep > -1) {
+
+ if ((robot.getAlternateTurretAngle() <48) && (robot.getAlternateTurretAngle() > 42)){
+ turretPower = 0;
+ robot.raiseCollector();
+ }
+ else{
+ turretPower = -turretPIDController.calculate(robot.getAlternateTurretAngle() - 45);
+ }
+ }
+ robot.setTurretPowerPct(turretPower);
+ switch (commandStep) { /////////////tapeAlign Code
+ case -1:
+ robot.resetEncoders();
+ robot.resetAttitude();
+ turretPIDController.disableContinuousInput();
+
+ if (System.currentTimeMillis() >= startingTime + 100) {
+ commandStep += 1;
+ }
+ break;
+ case 0:
+ startAngle = robot.getYaw();
+ startDistance = robot.getDriveDistanceInchesLeft();
+ PIDSteering.reset();
+ PIDSteering.enableContinuousInput(-180, 180);
+ commandStep += 1;//TODO:change back
+ break;
+ case 12: //TODO: tester case, remove when necessary
+ leftPower = defaultPower;
+ rightPower = defaultPower;
+ if (Math.abs(robot.getDriveDistanceInchesLeft() - startDistance) >= 12){
+ commandStep = 5;
+ leftPower = 0;
+ rightPower = 0;
+ }
+ break;
+ case 1:
+ correction = PIDSteering.calculate(robot.getYaw() - startAngle);
+ leftPower = defaultPower + correction; //didn't we stop doing this?
+ rightPower = defaultPower - correction;
+
+ if (!robot.getFloorSensorLeft() && !robot.getFloorSensorRight()){
+ Tapetheta = 0;
+ commandStep = 3;
+ }
+ else if (!robot.getFloorSensorLeft()) {
+ startDistance = robot.getDriveDistanceInchesLeft();
+ leftSensor = true;
+ commandStep += 1;
+ } else if (!robot.getFloorSensorRight()) {
+ startDistance = robot.getDriveDistanceInchesLeft();
+ rightSensor = true;
+ commandStep += 1;
+ }
+ break;
+ case 2:
+ correction = PIDSteering.calculate(robot.getYaw() - startAngle);
+ leftPower = defaultPower + correction; //confusion
+ rightPower = defaultPower - correction;
+
+ if (!rightSensor && !robot.getFloorSensorRight()) {
+ differenceDistance = Math.abs(robot.getDriveDistanceInchesLeft() - startDistance);
+ Tapetheta = (Math.atan(differenceDistance / sensorDist) * 180 / Math.PI);
+ commandStep += 1;
+ } else if (!leftSensor && !robot.getFloorSensorLeft()) {
+ differenceDistance = Math.abs(robot.getDriveDistanceInchesLeft() - startDistance);
+ Tapetheta = (Math.atan(differenceDistance / sensorDist) * 180 / Math.PI);
+ commandStep += 1;
+ }
+ break;
+
+ case 3:
+ robot.setArmsForward();
+ if (leftSensor) {
+ currentYaw = startAngle - Tapetheta; //currentYaw = targetYaw because we are lazy
+ } else {
+ currentYaw = startAngle + Tapetheta; //currentYaw = targetYaw because we are lazy
+ }
+ PIDSteering.reset();
+ PIDSteering.enableContinuousInput(-180, 180);
+ startDistance = robot.getDriveDistanceInchesLeft();
+ commandStep += 1;
+ break;
+ case 4:
+ correction = PIDSteering.calculate(robot.getYaw() - currentYaw);
+ leftPower = defaultPower + correction;
+ rightPower = defaultPower - correction;
+ if (Math.abs(robot.getDriveDistanceInchesLeft() - startDistance) >= (fwd-10)){
+ double ramp = rampDown(defaultPower, .1, startDistance, 10, robot.getDriveDistanceInchesLeft(), fwd);
+ leftPower = ramp;
+ rightPower = ramp;
+ }
+ if (Math.abs(robot.getDriveDistanceInchesLeft() - startDistance) >= (fwd)) {
+ leftPower = 0;
+ rightPower = 0;
+ commandStep += 1;
+ }
+ break;
+ case 5: //adios amigos
+ leftPower = 0;
+ rightPower = 0;
+ tapeAlign = false;
+ commandStep = -1;
+ startingTime = System.currentTimeMillis();
+ break;
+ /////////TODO:Put arms forward
+ }
+ robot.drivePercent(leftPower, rightPower);
+ }
+ else{////////////////////////////start the real stuff now
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ switch (commandStep){
+ case -1:
+ commandStep += 1;
+ break;
+ case 0:///reset and enable PTO
+ //reset encoders
+ robot.turnOnPTO();
+ robot.resetEncoders();
+ leftArmPower = 0;
+ rightArmPower = 0;
+ countLeft = 0;
+ countRight = 0;
+ if (System.currentTimeMillis() - startingTime >= 2000){
+ SmartDashboard.putNumber("driveOutputCurrent", robot.getDriveCurrent());
+ commandStep = 2; ///TODO: fix numbering
+ }
+
+ break;
+ /*case 1: ///////////unlock rotation piston to send arms forward
+ robot.setArmsForward(); //TODO: skip step
+ commandStep += 1;
+ break;*/
+ case 2: //////raise climber arms (skip 10 steps after in case we need to scoot/scoot
+
+ if (!robot.getClimbSensorLeft() && countLeft == 0){
+ countLeft = 1;
+ }
+
+ if (robot.getClimbSensorLeft() && countLeft == 1){
+ leftArmPower = 0;
+ leftArrived = true;
+ }
+ else{
+ leftArmPower = defaultClimbPowerUp;
+ }
+
+
+ if (!robot.getClimbSensorRight() && countRight == 0){
+ countRight = 1;
+ }
+
+ if (robot.getClimbSensorRight() && countRight == 1){
+ rightArmPower = 0;
+ rightArrived = true;
+ }
+ else{
+ rightArmPower = defaultClimbPowerUp;
+ }
+
+ if (leftArrived && rightArrived){
+ leftArrived = false;
+ rightArrived = false;
+ countLeft = 0;
+ countRight = 0;
+ leftArmPower = 0;
+ rightArmPower = 0;
+ startingTime = System.currentTimeMillis();
+ commandStep += 1;
+
+ }
+
+ break;
+ case 3: ///////////unlock rotation piston to send arms back
+ robot.setArmsBackward();
+ if (System.currentTimeMillis() - startingTime >= 1000) {
+ commandStep = 11;
+ }
+ break;
+ /*case 2: //////disable PTO
+ robot.turnOffPTO();
+ commandStep += 1;
+ break;
+ case 3: //////reset
+ startDistance = robot.getDriveDistanceInchesLeft();
+ commandStep += 1;
+ break;
+ case 4: //go back 8 in
+ leftArmPower = -.1; //see if we should change to drive stuff
+ rightArmPower = -.1;
+ if (Math.abs(robot.getDriveDistanceInchesLeft() - startDistance) >= 8){
+ leftArmPower = 0;
+ rightArmPower = 0;
+ commandStep += 1;
+ }
+ case 5: //////enable PTO
+ robot.turnOnPTO();
+ commandStep = 11;
+ break;*/
+ case 11: ////////lower climber arms
+
+ if (!robot.getClimbSensorLeft() && countLeft == 0){
+ countLeft = 1;
+ }
+
+ if (robot.getClimbSensorLeft() && countLeft == 1){
+ leftArmPower = 0;
+ leftArrived = true;
+ }
+ else{
+ leftArmPower = defaultClimbPowerDown;
+ }
+
+
+ if (!robot.getClimbSensorRight() && countRight == 0){
+ countRight = 1;
+ }
+
+ if (robot.getClimbSensorRight() && countRight == 1){
+ rightArmPower = 0;
+ rightArrived = true;
+ }
+ else{
+ rightArmPower = defaultClimbPowerDown;
+ }
+ if (robot.armInContact() && leftArrived && rightArrived){
+ countRight = 0;
+ countLeft = 0;
+ leftArmPower = 0;
+ rightArmPower = 0;
+ //TODO: change
+ commandStep += 1;
+ leftArrived = false;
+ rightArrived = false;
+ startHeightLeft = robot.armHeightLeft();
+ startHeightRight = robot.armHeightRight();
+ startingTime = System.currentTimeMillis();
+
+ }
+ break;
+
+ case 12: /////////////raise arms slightly
+ if (Math.abs(robot.armHeightLeft()-startHeightLeft) >= escapeHeight){
+ leftArmPower = 0;
+ leftArrived = true;
+ }
+ else{
+ leftArmPower = defaultClimbPowerUp;
+ }
+ if (Math.abs(robot.armHeightRight()-startHeightRight) >= escapeHeight){
+ rightArmPower = 0;
+ rightArrived = true;
+ }
+ else{
+ rightArmPower = defaultClimbPowerUp;
+ }
+ if (rightArrived && leftArrived){
+ rightArmPower = 0;
+ leftArmPower = 0;
+ rightArrived = false;
+ leftArrived = false;
+ commandStep += 1;
+ }
+ break;
+ case 13: ///////////unlock rotation piston to send arms forward
+ robot.setArmsForward();
+ commandStep += 1;
+ break;
+ case 14: ///////////move arms forward
+
+ if (!robot.getClimbSensorLeft() && countLeft == 0){
+ countLeft = 1;
+ }
+
+ if (robot.getClimbSensorLeft() && countLeft == 1){
+ leftArmPower = 0;
+ leftArrived = true;
+ }
+ else{
+ leftArmPower = defaultClimbPowerUp;
+ }
+
+
+ if (!robot.getClimbSensorRight() && countRight == 0){
+ countRight = 1;
+ }
+
+ if (robot.getClimbSensorRight() && countRight == 1){
+ rightArmPower = 0;
+ rightArrived = true;
+ }
+ else{
+ rightArmPower = defaultClimbPowerUp;
+ }
+
+ if (leftArrived && rightArrived){
+ countLeft = 0;
+ countRight = 0;
+ rightArmPower = 0;
+ leftArmPower = 0;
+ commandStep = 16; //////////skip over step 15
+ startingTime = System.currentTimeMillis();
+ }
+ break;
+ case 15:///change to check with pitch and roll
+ // actually don't even need this step :)
+ if (robot.getPitch() >= -10){
+ //if (robot.armInContact()){
+ commandStep += 1;
+ leftArmPower = 0;
+ rightArmPower = 0;
+ rightArrived = false;
+ leftArrived = false;
+ }
+ else{
+ leftArmPower = -.1;
+ rightArmPower = -.1;
+ }
+ case 16://///////once in contact move arms back again with the piston and swiiiiing
+ robot.setArmsBackward();
+ if (System.currentTimeMillis() - startingTime >= 1000) {
+ commandStep += 1;//TODO:change back
+ }
+ break;
+ case 17://////////go back to case 11 and repeat down to this step
+ if (firstTime){
+ commandStep = 11;
+ countRight = 0;
+ countLeft = 0;
+ rightArrived = false;
+ leftArrived = false;
+ firstTime = false;
+ }
+ else{
+ commandStep += 1;
+ leftArmPower = 0;
+ rightArrived = false;
+ leftArrived = false;
+ rightArmPower = 0;
+ }
+ break;
+ case 18:///////lift all the way up to be extra secure
+
+ if (!robot.getClimbSensorLeft() && countLeft == 0){
+ countLeft = 1;
+ }
+
+ if (robot.getClimbSensorLeft() && countLeft == 1){
+ leftArmPower = 0;
+ leftArrived = true;
+ }
+ else{
+ leftArmPower = defaultClimbPowerDown;
+ }
+
+
+ if (!robot.getClimbSensorRight() && countRight == 0){
+ countRight = 1;
+ }
+
+ if (robot.getClimbSensorRight() && countRight == 1){
+ rightArmPower = 0;
+ rightArrived = true;
+ }
+ else{
+ rightArmPower = defaultClimbPowerDown;
+ }
+
+ if (robot.armInContact() && leftArrived && rightArrived){
+ countRight = 0;
+ countLeft = 0;
+ leftArmPower = 0;
+ rightArmPower = 0;
+ leftArrived = false;
+ rightArrived = false;
+ commandStep += 1;
+ }
+ break;
+ case 19: ////////now we are done. If all goes well, we are on the traversal rung, if not, we no longer have a robot >;(
+ leftArmPower = 0;
+ rightArmPower = 0;
+ break;
+
+
+ }
+ if (robot.getRoll() - level > leveltol){
+ rightArmPower *= .8;
+ }
+ if (robot.getRoll() - level < - leveltol){
+ leftArmPower *= .8;
+ }
+ /*if (Math.abs(robot.getRoll()-level) >= 3*leveltol){
+ rightArmPower = 0;
+ leftArmPower = 0;
+ }*/
+ robot.armPower(leftArmPower, rightArmPower);
+ }
+ }
+}
diff --git a/src/main/java/frc/robot/command/HangWithoutAlign.java b/src/main/java/frc/robot/command/HangWithoutAlign.java
new file mode 100644
index 0000000..9286c0a
--- /dev/null
+++ b/src/main/java/frc/robot/command/HangWithoutAlign.java
@@ -0,0 +1,367 @@
+package frc.robot.command;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.generic.GenericRobot;
+
+public class HangWithoutAlign extends GenericCommand{
+
+
+ double startAngle;
+
+ double leftPower;
+ double rightPower;
+ double defaultPower = .4;
+
+ double startDistance;
+ double differenceDistance;
+
+ double sensorDist = 21.0;
+ double Tapetheta = 0;
+
+ double correction;
+ double currentYaw;
+
+ long startingTime = 0;
+
+ boolean leftSensor = false;
+ boolean rightSensor = false;
+
+ double lTraveled;
+
+ double fwd = 49.5;
+ PIDController PIDSteering;
+ boolean tapeAlign;
+
+ /////^^^^^^^^^^^Stuff for tapeAlign/
+
+
+ //////////////Now the real stuff
+ double escapeHeight = 10;///TODO:what is this??
+ boolean firstTime = true;
+ int countLeft = 0;
+ int countRight = 0;
+ double leftArmPower = 0;
+ double rightArmPower = 0;
+ double defaultClimbPowerUp = .75;
+ double defaultClimbPowerDown = -.75;
+ boolean leftArrived = false;
+ boolean rightArrived = false;
+ double startHeightLeft = 0;
+ double startHeightRight = 0;
+ double turretPower = 0;
+ double level = 7;
+ double leveltol = 2;
+
+ PIDController turretPIDController;
+
+
+ public void begin(GenericRobot robot){
+ startingTime = System.currentTimeMillis();
+ commandStep = -1;
+ leftSensor = false;
+ rightSensor = false;
+ leftArmPower = 0;
+ rightArmPower = 0;
+ leftPower = 0;
+ rightPower = 0;
+ lTraveled = 0;
+ fwd = 49.5;
+ PIDSteering = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD());
+ turretPIDController = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD());
+ tapeAlign = true;
+ firstTime = true;
+ }
+
+ public void step(GenericRobot robot){
+
+ SmartDashboard.putNumber("countLeft", countLeft);
+ SmartDashboard.putNumber("countRight", countRight);
+ SmartDashboard.putNumber("startDistance", startDistance);
+
+
+ switch (commandStep){
+ case -1:
+ commandStep += 1;
+ break;
+ case 0:///reset and enable PTO
+ //reset encoders
+ robot.turnOnPTO();
+ robot.resetEncoders();
+ leftArmPower = 0;
+ rightArmPower = 0;
+ countLeft = 0;
+ countRight = 0;
+ if (System.currentTimeMillis() - startingTime >= 5000){
+ SmartDashboard.putNumber("driveOutputCurrent", robot.getDriveCurrent());
+ commandStep = 2; ///TODO: fix numbering
+ }
+
+ break;
+ /*case 1: ///////////unlock rotation piston to send arms forward
+ robot.setArmsForward(); //TODO: skip step
+ commandStep += 1;
+ break;*/
+ case 2: //////raise climber arms (skip 10 steps after in case we need to scoot/scoot
+
+ if (!robot.getClimbSensorLeft() && countLeft == 0){
+ countLeft = 1;
+ }
+
+ if (robot.getClimbSensorLeft() && countLeft == 1){
+ leftArmPower = 0;
+ leftArrived = true;
+ }
+ else{
+ leftArmPower = defaultClimbPowerUp;
+ }
+
+
+ if (!robot.getClimbSensorRight() && countRight == 0){
+ countRight = 1;
+ }
+
+ if (robot.getClimbSensorRight() && countRight == 1){
+ rightArmPower = 0;
+ rightArrived = true;
+ }
+ else{
+ rightArmPower = defaultClimbPowerUp;
+ }
+
+ if (leftArrived && rightArrived){
+ leftArrived = false;
+ rightArrived = false;
+ countLeft = 0;
+ countRight = 0;
+ leftArmPower = 0;
+ rightArmPower = 0;
+ startingTime = System.currentTimeMillis();
+ commandStep += 1;
+
+ }
+
+ break;
+ case 3: ///////////unlock rotation piston to send arms back
+ robot.setArmsBackward();
+ if (System.currentTimeMillis() - startingTime >= 1000) {
+ commandStep = 11;
+ }
+ break;
+ /*case 2: //////disable PTO
+ robot.turnOffPTO();
+ commandStep += 1;
+ break;
+ case 3: //////reset
+ startDistance = robot.getDriveDistanceInchesLeft();
+ commandStep += 1;
+ break;
+ case 4: //go back 8 in
+ leftArmPower = -.1; //see if we should change to drive stuff
+ rightArmPower = -.1;
+ if (Math.abs(robot.getDriveDistanceInchesLeft() - startDistance) >= 8){
+ leftArmPower = 0;
+ rightArmPower = 0;
+ commandStep += 1;
+ }
+ case 5: //////enable PTO
+ robot.turnOnPTO();
+ commandStep = 11;
+ break;*/
+ case 11: ////////lower climber arms
+
+ if (!robot.getClimbSensorLeft() && countLeft == 0){
+ countLeft = 1;
+ }
+
+ if (robot.getClimbSensorLeft() && countLeft == 1){
+ leftArmPower = 0;
+ leftArrived = true;
+ }
+ else{
+ leftArmPower = defaultClimbPowerDown;
+ }
+
+
+ if (!robot.getClimbSensorRight() && countRight == 0){
+ countRight = 1;
+ }
+
+ if (robot.getClimbSensorRight() && countRight == 1){
+ rightArmPower = 0;
+ rightArrived = true;
+ }
+ else{
+ rightArmPower = defaultClimbPowerDown;
+ }
+ if (robot.armInContact() && leftArrived && rightArrived){
+ countRight = 0;
+ countLeft = 0;
+ leftArmPower = 0;
+ rightArmPower = 0;
+ //TODO: change
+ commandStep += 1;
+ leftArrived = false;
+ rightArrived = false;
+ startHeightLeft = robot.armHeightLeft();
+ startHeightRight = robot.armHeightRight();
+ startingTime = System.currentTimeMillis();
+
+ }
+ break;
+
+ case 12: /////////////raise arms slightly
+ if (Math.abs(robot.armHeightLeft()-startHeightLeft) >= escapeHeight){
+ leftArmPower = 0;
+ leftArrived = true;
+ }
+ else{
+ leftArmPower = defaultClimbPowerUp;
+ }
+ if (Math.abs(robot.armHeightRight()-startHeightRight) >= escapeHeight){
+ rightArmPower = 0;
+ rightArrived = true;
+ }
+ else{
+ rightArmPower = defaultClimbPowerUp;
+ }
+ if (rightArrived && leftArrived){
+ rightArmPower = 0;
+ leftArmPower = 0;
+ rightArrived = false;
+ leftArrived = false;
+ commandStep += 1;
+ }
+ break;
+ case 13: ///////////unlock rotation piston to send arms forward
+ robot.setArmsForward();
+ commandStep += 1;
+ break;
+ case 14: ///////////move arms forward
+
+ if (!robot.getClimbSensorLeft() && countLeft == 0){
+ countLeft = 1;
+ }
+
+ if (robot.getClimbSensorLeft() && countLeft == 1){
+ leftArmPower = 0;
+ leftArrived = true;
+ }
+ else{
+ leftArmPower = defaultClimbPowerUp;
+ }
+
+
+ if (!robot.getClimbSensorRight() && countRight == 0){
+ countRight = 1;
+ }
+
+ if (robot.getClimbSensorRight() && countRight == 1){
+ rightArmPower = 0;
+ rightArrived = true;
+ }
+ else{
+ rightArmPower = defaultClimbPowerUp;
+ }
+
+ if (leftArrived && rightArrived){
+ countLeft = 0;
+ countRight = 0;
+ rightArmPower = 0;
+ leftArmPower = 0;
+ commandStep = 16; //////////skip over step 15
+ startingTime = System.currentTimeMillis();
+ }
+ break;
+ case 15:///change to check with pitch and roll
+ // actually don't even need this step :)
+ if (robot.getPitch() >= -10){
+ //if (robot.armInContact()){
+ commandStep += 1;
+ leftArmPower = 0;
+ rightArmPower = 0;
+ rightArrived = false;
+ leftArrived = false;
+ }
+ else{
+ leftArmPower = -.1;
+ rightArmPower = -.1;
+ }
+ case 16://///////once in contact move arms back again with the piston and swiiiiing
+ robot.setArmsBackward();
+ if (System.currentTimeMillis() - startingTime >= 1000) {
+ commandStep += 1;//TODO:change back
+ }
+ break;
+ case 17://////////go back to case 11 and repeat down to this step
+ if (firstTime){
+ commandStep = 11;
+ countRight = 0;
+ countLeft = 0;
+ rightArrived = false;
+ leftArrived = false;
+ firstTime = false;
+ }
+ else{
+ commandStep += 1;
+ leftArmPower = 0;
+ rightArrived = false;
+ leftArrived = false;
+ rightArmPower = 0;
+ }
+ break;
+ case 18:///////lift all the way up to be extra secure
+
+ if (!robot.getClimbSensorLeft() && countLeft == 0){
+ countLeft = 1;
+ }
+
+ if (robot.getClimbSensorLeft() && countLeft == 1){
+ leftArmPower = 0;
+ leftArrived = true;
+ }
+ else{
+ leftArmPower = defaultClimbPowerDown;
+ }
+
+
+ if (!robot.getClimbSensorRight() && countRight == 0){
+ countRight = 1;
+ }
+
+ if (robot.getClimbSensorRight() && countRight == 1){
+ rightArmPower = 0;
+ rightArrived = true;
+ }
+ else{
+ rightArmPower = defaultClimbPowerDown;
+ }
+
+ if (robot.armInContact() && leftArrived && rightArrived){
+ countRight = 0;
+ countLeft = 0;
+ leftArmPower = 0;
+ rightArmPower = 0;
+ leftArrived = false;
+ rightArrived = false;
+ commandStep += 1;
+ }
+ break;
+ case 19: ////////now we are done. If all goes well, we are on the traversal rung, if not, we no longer have a robot >;(
+ leftArmPower = 0;
+ rightArmPower = 0;
+ break;
+
+
+ }
+ if (robot.getRoll() - level > leveltol){
+ rightArmPower *= .8;
+ }
+ if (robot.getRoll() - level < - leveltol){
+ leftArmPower *= .8;
+ }
+
+ robot.armPower(leftArmPower, rightArmPower);
+ }
+
+}
diff --git a/src/main/java/frc/robot/generic/Camoelot.java b/src/main/java/frc/robot/generic/Camoelot.java
index 913e0bc..f18fa1e 100644
--- a/src/main/java/frc/robot/generic/Camoelot.java
+++ b/src/main/java/frc/robot/generic/Camoelot.java
@@ -155,12 +155,12 @@ public double encoderRightDriveTicksPerInch(){
}
@Override
- public double encoderTicksLeftDrive(){
+ public double encoderTicksLeftDriveA(){
return leftEncoder.get();
}
@Override
- public double encoderTicksRightDrive(){
+ public double encoderTicksRightDriveA(){
return rightEncoder.get();
}
diff --git a/src/main/java/frc/robot/generic/Falcon.java b/src/main/java/frc/robot/generic/Falcon.java
index 323a871..a154098 100644
--- a/src/main/java/frc/robot/generic/Falcon.java
+++ b/src/main/java/frc/robot/generic/Falcon.java
@@ -2,9 +2,6 @@
import com.kauailabs.navx.frc.AHRS;
import com.revrobotics.*;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.AnalogInput;
@@ -136,12 +133,12 @@ public double getDriveRightRPM() {
@Override
public double getDriveDistanceInchesLeft() {
- return encoderTicksLeftDrive()/encoderLeftDriveTicksPerInch();
+ return encoderTicksLeftDriveA()/encoderLeftDriveTicksPerInch();
}
@Override
public double getDriveDistanceInchesRight() {
- return encoderTicksRightDrive()/encoderRightDriveTicksPerInch();
+ return encoderTicksRightDriveA()/encoderRightDriveTicksPerInch();
}
@Override
@@ -155,12 +152,12 @@ public double encoderRightDriveTicksPerInch() {
}
@Override
- public double encoderTicksLeftDrive() {
+ public double encoderTicksLeftDriveA() {
return encoderLeft.getPosition();
}
@Override
- public double encoderTicksRightDrive() {
+ public double encoderTicksRightDriveA() {
return encoderRight.getPosition();
}
diff --git a/src/main/java/frc/robot/generic/GenericRobot.java b/src/main/java/frc/robot/generic/GenericRobot.java
index 4139de6..6d11af5 100644
--- a/src/main/java/frc/robot/generic/GenericRobot.java
+++ b/src/main/java/frc/robot/generic/GenericRobot.java
@@ -12,6 +12,10 @@ public interface GenericRobot {
NetworkTableEntry ta = table.getEntry("ta");
NetworkTableEntry tv = table.getEntry("tv");
+ public default void setPipeline(int id)
+ {
+ table.getEntry("pipeline").setNumber(id);
+ }
public void drivePercent(
double leftPercent,
@@ -41,11 +45,24 @@ public default double getDriveRightRPM(){
};
public default double getDriveDistanceInchesLeft(){
- return encoderTicksLeftDrive()/encoderLeftDriveTicksPerInch();
+ return encoderTicksLeftDriveA()/encoderLeftDriveTicksPerInch();
}
public default double getDriveDistanceInchesRight(){
- return encoderTicksRightDrive()/encoderRightDriveTicksPerInch();
+ return encoderTicksRightDriveB()/encoderRightDriveTicksPerInch();
+ }
+
+ public default double getLeftACurrent(){
+ return 0.0;
+ }
+ public default double getLeftBCurrent(){
+ return 0.0;
+ }
+ public default double getRightACurrent(){
+ return 0.0;
+ }
+ public default double getRightBCurrent(){
+ return 0.0;
}
public default double encoderLeftDriveTicksPerInch(){
@@ -58,16 +75,24 @@ public default double encoderRightDriveTicksPerInch(){
return 1.0;
}
- public default double encoderTicksLeftDrive(){
+ public default double encoderTicksLeftDriveA(){
//System.out.println("I don't have an encoder");
return 0;
}
- public default double encoderTicksRightDrive(){
+ public default double encoderTicksRightDriveA(){
//System.out.println("I don't have an encoder");
return 0;
}
+ public default double encoderTicksLeftDriveB(){
+ return 0;
+ }
+
+ public default double encoderTicksRightDriveB(){
+ return 0;
+ }
+
public default double getYaw(){
//System.out.println("I don't have a navX");
return 0;
@@ -108,12 +133,24 @@ public default void resetAttitude() {
System.out.println("I don't have a navx");
}
+
public enum TeamColor {
RED,
BLUE,
UNKNOWN;
}
+ public default void getCargo(){
+
+ }
+ public default void shoot(){
+
+ }
+
+ public default boolean canShoot(){
+ return false;
+ }
+
public default boolean getUpperCargo(){
//System.out.println("robot is colorblind");
return false;
@@ -129,7 +166,7 @@ public default void setCollectorIntakePercentage(
}
public default double getCollectorIntakePercentage() {
return 0;
- };
+ }
public void setIndexerIntakePercentage(
double percentage
@@ -200,9 +237,7 @@ public default double getTurretAngleDegrees(){
public default double getAlternateTurretAngle(){
return 0;
}
- public default double getAlternateTurretAngleDegrees(){
- return getAlternateTurretAngle()/encoderTurretTicksPerDegree();
- }
+
public default void setTurretAngleRelative(double angleChange){
@@ -219,6 +254,10 @@ public default double getTurretPowerPct(){
return 0;
}
+ public default double getTurretPitchPosition(){
+ return 0;
+ }
+
public default double getTurretPitchAngle(){
return 0;
}
@@ -226,6 +265,10 @@ public default double getTurretPitchPowerPct(){
return 0;
}
+ public default void setTurretPitchPosition(double position){
+ //System.out.println("I don't have a turret");
+ }
+
public default void setTurretPitchAngle(){
//System.out.println("I don't have a turret");
}
@@ -253,6 +296,8 @@ public default double getShooterTargetHeight(){
}
public default double getShooterTargetRPM() { return 0; }
+ public default void setShooterTargetRPM(double rpm){}
+
public default void setShooterRPM(double topRPM, double bottomRPM){
//System.out.println("I don't have a shooter");
}
@@ -305,6 +350,10 @@ public default void setArmsBackward() {
//System.out.println("I don't have a climber");
}
+ public default void armPower(double powerOne, double powerTwo){
+
+ }
+
//returns true if PTO set to arms, return false if PTO set to drive
public default boolean getPTOState(){
return false;
@@ -338,8 +387,12 @@ public default boolean isReadyToShoot(double tolerance, double time){
if(error > tolerance) shooterNotReady();
//we haven't called shooterNotReady() in the last "time" milliseconds
- if(System.currentTimeMillis() - getShootReadyTimer() > time) return true;
- return false;
+ if(System.currentTimeMillis() - getShootReadyTimer() > time) {
+ return true;
+ }
+ else{
+ return false;
+ }
}
public default boolean isActivelyShooting(){
@@ -348,4 +401,38 @@ public default boolean isActivelyShooting(){
public default void setActivelyShooting(boolean isShooting){
}
+ public default void raiseClimberArms(double rightPower, double leftPower){
+ //System.out.println("I don't have a climber");
+ }
+ public default void lowerClimberArms(double rightPower, double leftPower){
+ //System.out.println("I don't have a climber");
+ }
+ public default void armPower(double power){
+ //System.out.println("Elbow Grease");
+ }
+
+ public default double armHeightLeft(){
+ return 0.0;
+ //System.out.println("I don't have a climber");
+ }
+ public default double armHeightRight(){
+ return 0.0;
+ }
+
+
+ public default boolean armInContact(){
+ return false;
+ //System.out.println("I don't have a climber");
+ }
+
+ public default boolean inTheRightPlace(){
+ return false;
+ //System.out.println("I don't have a climber");
+ }
+
+ public default double getDriveCurrent(){
+ return 0.0;
+ }
+
+
}
diff --git a/src/main/java/frc/robot/generic/Lightning.java b/src/main/java/frc/robot/generic/Lightning.java
index fe79b42..5f57c84 100644
--- a/src/main/java/frc/robot/generic/Lightning.java
+++ b/src/main/java/frc/robot/generic/Lightning.java
@@ -7,11 +7,16 @@
import static com.revrobotics.CANSparkMaxLowLevel.MotorType.kBrushless;
public class Lightning implements GenericRobot {
- public static final double TICKS_PER_INCH_DRIVE = 0.96;
+ public static final double TICKS_PER_INCH_DRIVE = 0.875;
public static final double TICKS_PER_DEGREE_TURRET = 116;
public static final double TICKS_PER_DEGREE_TURRET2 = 136.467;
- public static final double TICKS_PER_REVOLUTION_SHOOTERA = 116;
- public static final double TICKS_PER_REVOLUTION_SHOOTERB = 116;
+ public static final double TICKS_PER_REVOLUTION_SHOOTERA = 1;
+ public static final double TICKS_PER_REVOLUTION_SHOOTERB = 1;
+
+ public static final double LEFTATOLERANCE = 0;
+ public static final double LEFTBTOLERANCE = 0;
+ public static final double RIGHTATOLERANCE = 0;
+ public static final double RIGHTBTOLERANCE = 0;
AHRS navx = new AHRS(SPI.Port.kMXP, (byte) 50);
@@ -26,19 +31,26 @@ public class Lightning implements GenericRobot {
CANSparkMax rightMotorA = new CANSparkMax(18, kBrushless);
CANSparkMax rightMotorB = new CANSparkMax(19, kBrushless);
+ //TODO: update servo ports
+ //servo left was initially set to channel 9, don't know if that means anything
Servo elevationLeft = new Servo(9);
+ Servo elevationRight = new Servo(8);
+
- RelativeEncoder encoderRight = rightMotorA.getEncoder();
- RelativeEncoder encoderLeft = leftMotorA.getEncoder();
+ RelativeEncoder encoderRightA = rightMotorA.getEncoder();
+ RelativeEncoder encoderLeftA = leftMotorA.getEncoder();
+ RelativeEncoder encoderRightB = rightMotorB.getEncoder();
+ RelativeEncoder encoderLeftB = leftMotorB.getEncoder();
RelativeEncoder encoderTurret = turretRotator.getEncoder();
SparkMaxAnalogSensor encoderTurretAlt = turretRotator.getAnalog(SparkMaxAnalogSensor.Mode.kAbsolute);
RelativeEncoder encoderShootA = shooterA.getEncoder();
RelativeEncoder encoderShootB = shooterB.getEncoder();
+
//DigitalInput ballSensor = new DigitalInput(0);
DoubleSolenoid PTO = new DoubleSolenoid(PneumaticsModuleType.CTREPCM, 4, 5);
DoubleSolenoid arms = new DoubleSolenoid(PneumaticsModuleType.CTREPCM, 1, 2);
- Solenoid collectorPosition = new Solenoid(PneumaticsModuleType.CTREPCM, 0);
+ DoubleSolenoid collectorPosition = new DoubleSolenoid(PneumaticsModuleType.CTREPCM, 6,7);
SparkMaxPIDController shooterAPIDController = shooterA.getPIDController();
SparkMaxPIDController shooterBPIDController = shooterB.getPIDController();
@@ -54,13 +66,19 @@ public class Lightning implements GenericRobot {
SparkMaxLimitSwitch limitSwitchLeftBForward = leftMotorB.getForwardLimitSwitch(lstype);
SparkMaxLimitSwitch limitSwitchLeftBReverse = leftMotorB.getReverseLimitSwitch(lstype);
+ double defaultShooterTargetRPM = 4000;
+
boolean isPTOonArms;
+
//True = robot is in the process of and committed to shooting a cargo at mach 12
boolean isActivelyShooting;
+ boolean canShoot;
//shootReadyTimer is used to check if shooter ready
long shootReadyTimer;
+ double attemptedRPM = 0;
+
public Lightning(){
boolean invertLeft = false;
boolean invertRight = true;
@@ -69,12 +87,36 @@ public Lightning(){
rightMotorA.setInverted(invertRight);
rightMotorB.setInverted(invertRight);
+ turretRotator.setIdleMode(CANSparkMax.IdleMode.kBrake);
+
+ leftMotorA.setIdleMode(CANSparkMax.IdleMode.kBrake);
+ leftMotorB.setIdleMode(CANSparkMax.IdleMode.kBrake);
+ rightMotorA.setIdleMode(CANSparkMax.IdleMode.kBrake);
+ rightMotorB.setIdleMode(CANSparkMax.IdleMode.kBrake);
+
indexer.setInverted(true);
collector.setInverted(false);
- shooterB.setInverted(false);
+ //shooterB.setInverted(false);
shooterA.setInverted(true);
+ shooterB.follow(shooterA, true);
+
elevationLeft.setBounds(2.0, 1.8, 1.5, 1.2, 1.0);
+ elevationRight.setBounds(2.0, 1.8, 1.5, 1.2, 1.0);
+
+ shooterAPIDController.setP(5.0e-4);
+ shooterAPIDController.setI(5.0e-7);
+ shooterAPIDController.setD(5.0e-1);
+ shooterAPIDController.setFF(1.7e-4);
+ shooterAPIDController.getIZone(500);
+ shooterAPIDController.getDFilter(0);
+
+ /*shooterBPIDController.setP(5.0e-4);
+ shooterBPIDController.setI(5.0e-7);
+ shooterBPIDController.setD(5.0e-1);
+ shooterBPIDController.setFF(1.7e-4);
+ shooterBPIDController.getIZone(500);
+ shooterBPIDController.getDFilter(0);*/
shootReadyTimer = System.currentTimeMillis();
@@ -83,6 +125,9 @@ public Lightning(){
indexer.setIdleMode(CANSparkMax.IdleMode.kBrake);
+ shooterA.setIdleMode(CANSparkMax.IdleMode.kCoast);
+ shooterB.setIdleMode(CANSparkMax.IdleMode.kCoast);
+
limitSwitchIndexerForward.enableLimitSwitch(false);
limitSwitchIndexerReverse.enableLimitSwitch(false);
limitSwitchRightAForward.enableLimitSwitch(false);
@@ -91,6 +136,10 @@ public Lightning(){
limitSwitchLeftBForward.enableLimitSwitch(false);
limitSwitchLeftBReverse.enableLimitSwitch(false);
+ elevationLeft.setBounds(2.0, 1.8, 1.5, 1.2, 1.0);
+ elevationRight.setBounds(2.0, 1.8, 1.5, 1.2, 1.0);
+
+
}
@Override
@@ -113,14 +162,33 @@ public double getDriveRightPercentage() {
@Override
public double getDriveLeftRPM() {
- return encoderLeft.getVelocity()/encoderLeftDriveTicksPerInch();
+ return encoderLeftA.getVelocity()/encoderLeftDriveTicksPerInch();
}
@Override
public double getDriveRightRPM() {
- return encoderRight.getVelocity()/encoderRightDriveTicksPerInch();
+ return encoderRightA.getVelocity()/encoderRightDriveTicksPerInch();
+ }
+
+ @Override
+ public double getLeftACurrent(){
+ return leftMotorA.getOutputCurrent();
+ }
+
+ @Override
+ public double getLeftBCurrent(){
+ return leftMotorB.getOutputCurrent();
+ }
+
+ @Override
+ public double getRightACurrent(){
+ return rightMotorA.getOutputCurrent();
}
+ @Override
+ public double getRightBCurrent(){
+ return rightMotorB.getOutputCurrent();
+ }
@Override
public double encoderLeftDriveTicksPerInch() {
@@ -133,13 +201,23 @@ public double encoderRightDriveTicksPerInch() {
}
@Override
- public double encoderTicksLeftDrive() {
- return encoderLeft.getPosition();
+ public double encoderTicksLeftDriveA() {
+ return encoderLeftA.getPosition();
+ }
+
+ @Override
+ public double encoderTicksRightDriveA() {
+ return encoderRightA.getPosition();
}
@Override
- public double encoderTicksRightDrive() {
- return encoderRight.getPosition();
+ public double encoderTicksLeftDriveB(){
+ return encoderLeftB.getPosition();
+ }
+
+ @Override
+ public double encoderTicksRightDriveB(){
+ return encoderRightB.getPosition();
}
@Override
@@ -149,12 +227,12 @@ public double getYaw() {
@Override
public double getPitch() {
- return navx.getPitch();
+ return navx.getRoll();
}
@Override
public double getRoll() {
- return navx.getRoll();
+ return -navx.getPitch();
}
@Override
@@ -166,6 +244,43 @@ public boolean getLowerCargo() {
return limitSwitchIndexerForward.isPressed();
}
+ @Override
+ public void getCargo(){
+ double collectorPct = 1;
+ double indexerPct = 1;
+ if (getUpperCargo()){
+ if (isActivelyShooting){
+ indexerPct = 1;
+ }
+ else{
+ indexerPct = 0;
+ }
+ if (getLowerCargo()){
+ collectorPct = 0;
+ }
+ }
+ setIndexerIntakePercentage(indexerPct);
+ setCollectorIntakePercentage(collectorPct);
+
+ }
+
+ @Override
+ public void shoot(){
+ double shooterRPM = defaultShooterTargetRPM;
+ setShooterRPM(shooterRPM, shooterRPM);
+ if (getShooterRPMTop() >= (shooterRPM-300) && getShooterRPMBottom() >= (shooterRPM-300)){
+ canShoot = true;
+ }
+ else{
+ canShoot = false;
+ }
+ }
+
+ @Override
+ public boolean canShoot(){
+ return canShoot;
+ }
+
@Override
public void setCollectorIntakePercentage(double percentage) {
@@ -214,10 +329,29 @@ public double encoderTurretTicksPerDegree(){
}
//Measured range 0.042 - 2.68
+
+ /*
+ Offset is to set the turret encoder origin. It is the difference between a desired turret angle and the actual one.
+ Sample calculation:
+ Point the turret straight forward which is supposed to be 045.
+ Read the Alternate Turret Angle Degrees on the smart dashboard. Suppose it reads "52.5"
+ Offset = 52.5-45 = 12.5
+ */
@Override
public double getAlternateTurretAngle(){
double raw = encoderTurretAlt.getPosition();
- return (raw * 136.467) - 5.73;
+ double out;
+ double offset = 78 + 15;
+ out = (raw * 136.467) - 5.73 - offset;
+ if (out>360)
+ {
+ out = out-360;
+ }
+ else if (out<0)
+ {
+ out = out+360;
+ }
+ return (out);
}
@Override
@@ -236,7 +370,15 @@ public void setTurretAngleAbsolute() {
@Override
public void setTurretPowerPct(double powerPct) {
- turretRotator.set(powerPct);
+ if ( (getAlternateTurretAngle()>350) & (powerPct<0))
+ {
+ powerPct = 0;
+ }
+ if ( (getAlternateTurretAngle()<10) & (powerPct>0))
+ {
+ powerPct = 0;
+ }
+ turretRotator.set(-powerPct);
}
@Override
@@ -245,8 +387,20 @@ public double getTurretPowerPct() {
}
@Override
- public void setTurretPitchPowerPct(double speed){
- elevationLeft.setSpeed(speed);
+ public double getTurretPitchPosition(){
+
+ //TODO: getSpeed()? getPosition()? getAngle()? don't know which to use
+ return elevationLeft.get();
+ }
+ @Override
+ public void setTurretPitchPosition(double position){
+ if(position < 0) position = 0;
+ if(position > 1) position = 1;
+
+
+ //TODO: figure out use setSpeed() or set()
+ elevationLeft.set(position);
+ elevationRight.set(position);
}
@@ -285,18 +439,24 @@ public double getShooterTargetHeight() {
@Override
public void setShooterRPM(double topRPM, double bottomRPM) {
+ attemptedRPM = topRPM;
setShooterRPMTop(topRPM);
- setShooterRPMBottom(bottomRPM);
+ //setShooterRPMBottom(bottomRPM);
}
@Override
public void setShooterRPMTop(double rpm) {
+ if (rpm < 10){
+ shooterA.set(0);
+ }
+ else{
shooterAPIDController.setReference(rpm, CANSparkMax.ControlType.kVelocity);
+ }
}
@Override
public void setShooterRPMBottom(double rpm) {
- shooterBPIDController.setReference(rpm, CANSparkMax.ControlType.kVelocity);
+ shooterBPIDController.setReference(rpm, CANSparkMax.ControlType.kVelocity);//
}
@@ -325,7 +485,12 @@ public void setShooterTargetDistance(double length, double height) {
@Override
public double getShooterTargetRPM(){
- return 1000;
+ return defaultShooterTargetRPM;
+ }
+
+ @Override
+ public void setShooterTargetRPM(double rpm){
+ defaultShooterTargetRPM = rpm;
}
@Override
@@ -341,12 +506,12 @@ public boolean getFloorSensorRight(){
@Override
public void raiseCollector(){
- collectorPosition.set(true);
+ collectorPosition.set(DoubleSolenoid.Value.kForward);
}
@Override
public void lowerCollector(){
- collectorPosition.set(false);
+ collectorPosition.set(DoubleSolenoid.Value.kReverse);
}
//CONNECTS MOTORS TO CLIMB ARMS
@@ -377,6 +542,58 @@ public void setArmsBackward(){
arms.set(DoubleSolenoid.Value.kForward);
}
+ @Override
+ public void armPower(double leftPower, double rightPower) {
+ leftMotorB.set(leftPower);
+ leftMotorA.set(leftPower);
+ rightMotorA.set(rightPower);
+ rightMotorB.set(rightPower);
+ }
+ @Override
+ public void raiseClimberArms(double rightPower, double leftPower){
+ //System.out.println("I don't have a climber");
+ armPower(leftPower, rightPower);
+ }
+ @Override
+ public void lowerClimberArms(double rightPower, double leftPower){
+ //System.out.println("I don't have a climber");
+ armPower(-leftPower, -rightPower);
+ }
+
+ @Override
+ public double armHeightLeft() {
+ //TODO: put in conversion
+ //Maybe use some sensor. Do NOT want to use encoders for this.
+ return -encoderTicksLeftDriveA()*.24937;
+ }
+
+ @Override
+ public double armHeightRight(){
+ //TODO: put in conversion
+ return -encoderTicksRightDriveA()*.24937;
+ }
+
+ @Override
+ public boolean armInContact() {
+ //TODO: find tolerances
+ /* if (leftMotorA.getOutputCurrent() > LEFTATOLERANCE && leftMotorB.getOutputCurrent() > LEFTBTOLERANCE
+ && rightMotorA.getOutputCurrent() > RIGHTATOLERANCE && rightMotorB.getOutputCurrent() > RIGHTBTOLERANCE){
+ return true;
+ }
+ else{
+ return false;
+ }*/
+ return true;
+ }
+
+
+
+ @Override
+ public boolean inTheRightPlace(){
+ //TODO: maybe a sensor??
+ return false;
+ }
+
@Override
public boolean getClimbSensorLeft(){
return limitSwitchLeftBReverse.isPressed();
@@ -388,32 +605,32 @@ public boolean getClimbSensorRight(){
@Override
public double getPIDmaneuverP() {
- return GenericRobot.super.getPIDmaneuverP();
+ return 1.0e-2;
}
@Override
public double getPIDmaneuverI() {
- return GenericRobot.super.getPIDmaneuverI();
+ return 0;
}
@Override
public double getPIDmaneuverD() {
- return GenericRobot.super.getPIDmaneuverD();
+ return 1.0e-4;
}
@Override
public double getPIDpivotP() {
- return GenericRobot.super.getPIDpivotP();
+ return 1.5e-2;
}
@Override
public double getPIDpivotI() {
- return GenericRobot.super.getPIDpivotI();
+ return 0;
}
@Override
public double getPIDpivotD() {
- return GenericRobot.super.getPIDpivotD();
+ return 0;
}
@Override
@@ -429,19 +646,20 @@ public void resetAttitude() {
navx.reset();
}
+
@Override
public double turretPIDgetP() {
- return GenericRobot.super.turretPIDgetP();
+ return 2.0e-2;
}
@Override
public double turretPIDgetI() {
- return GenericRobot.super.turretPIDgetI();
+ return 0;
}
@Override
public double turretPIDgetD() {
- return GenericRobot.super.turretPIDgetD();
+ return 1.0e-3;
}
@@ -460,8 +678,18 @@ public boolean isActivelyShooting(){
}
//TODO: Add check using isReadyToShoot() function?
+ @Override
+ public boolean isReadyToShoot(){
+ return true;
+ }
+
@Override
public void setActivelyShooting(boolean isShooting){
isActivelyShooting = isShooting;
}
+
+ @Override
+ public double getDriveCurrent(){
+ return leftMotorA.getOutputCurrent();
+ }
}
diff --git a/src/main/java/frc/robot/generic/TurretBot.java b/src/main/java/frc/robot/generic/TurretBot.java
index ef85539..4417546 100644
--- a/src/main/java/frc/robot/generic/TurretBot.java
+++ b/src/main/java/frc/robot/generic/TurretBot.java
@@ -4,11 +4,6 @@
import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkMaxPIDController;
import com.revrobotics.CANSparkMax;
-import com.revrobotics.CANSparkMax.ControlType;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.*;
import static com.revrobotics.CANSparkMax.IdleMode.kBrake;
@@ -108,12 +103,12 @@ public double getDriveRightRPM() {
@Override
public double getDriveDistanceInchesLeft() {
- return encoderTicksLeftDrive() / encoderLeftDriveTicksPerInch();
+ return encoderTicksLeftDriveA() / encoderLeftDriveTicksPerInch();
}
@Override
public double getDriveDistanceInchesRight() {
- return encoderTicksRightDrive() / encoderRightDriveTicksPerInch();
+ return encoderTicksRightDriveA() / encoderRightDriveTicksPerInch();
}
@Override
@@ -127,12 +122,12 @@ public double encoderRightDriveTicksPerInch() {
}
@Override
- public double encoderTicksLeftDrive() {
+ public double encoderTicksLeftDriveA() {
return encoderLeft.getPosition();
}
@Override
- public double encoderTicksRightDrive() {
+ public double encoderTicksRightDriveA() {
return encoderRight.getPosition();
}