From 938deb3896057598fded1a405ff43b9dcb2b37c6 Mon Sep 17 00:00:00 2001 From: DeathDIVer Date: Mon, 14 Feb 2022 20:31:06 -0500 Subject: [PATCH 01/72] Started work on three ball routines and fixed some things in simpleA --- .idea/misc.xml | 2 +- .../frc/robot/autonomous/SimpleATerminal.java | 7 +- .../autonomous/SimpleAtoBtoTerminal.java | 151 ++++++++++++++++++ .../autonomous/SimpleCtoBtoTerminal.java | 4 + 4 files changed, 159 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/autonomous/SimpleAtoBtoTerminal.java create mode 100644 src/main/java/frc/robot/autonomous/SimpleCtoBtoTerminal.java diff --git a/.idea/misc.xml b/.idea/misc.xml index b808dca..7c92700 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -4,5 +4,5 @@ - + \ No newline at end of file diff --git a/src/main/java/frc/robot/autonomous/SimpleATerminal.java b/src/main/java/frc/robot/autonomous/SimpleATerminal.java index e71cf81..06265b1 100644 --- a/src/main/java/frc/robot/autonomous/SimpleATerminal.java +++ b/src/main/java/frc/robot/autonomous/SimpleATerminal.java @@ -29,10 +29,9 @@ public class SimpleATerminal extends GenericAutonomous { @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()); - } @Override @@ -66,7 +65,7 @@ public void autonomousPeriodic(GenericRobot robot) { if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceA){ autonomousStep += 1; startTime = System.currentTimeMillis(); - } //has 3 inches of momentum with .25 power + } break; case 5: //stop leftpower = 0; @@ -108,7 +107,7 @@ public void autonomousPeriodic(GenericRobot robot) { autonomousStep += 1; leftpower = 0; rightpower = 0; - } //might need to tune for momentum + } break; case 14: leftpower = 0; diff --git a/src/main/java/frc/robot/autonomous/SimpleAtoBtoTerminal.java b/src/main/java/frc/robot/autonomous/SimpleAtoBtoTerminal.java new file mode 100644 index 0000000..fd69f41 --- /dev/null +++ b/src/main/java/frc/robot/autonomous/SimpleAtoBtoTerminal.java @@ -0,0 +1,151 @@ +package frc.robot.autonomous; + +import edu.wpi.first.math.controller.PIDController; +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 SimpleAtoBtoTerminal extends GenericAutonomous { + double startingYaw; + double startTime; + double startDistance; + + double leftpower; + double rightpower; + double defaultPower = .25; + double defaultTurnPower = .25; + double correction; + + double distanceA = 37; + double distanceB = 117.1; + double distanceC = 160.6; + double angleA = 87.74; + double angleB = 0; //NEED TO ASK CAD FOR ANGLE + 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 >= distanceC - rampDownDist){ + defaultPower = (distanceC-robot.getDriveDistanceInchesLeft()+startDistance)*defaultPower/rampDownDist; + } + if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceC) { + 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/SimpleCtoBtoTerminal.java b/src/main/java/frc/robot/autonomous/SimpleCtoBtoTerminal.java new file mode 100644 index 0000000..84b02d0 --- /dev/null +++ b/src/main/java/frc/robot/autonomous/SimpleCtoBtoTerminal.java @@ -0,0 +1,4 @@ +package frc.robot.autonomous; + +public class SimpleCtoBtoTerminal extends GenericAutonomous{ +} From 01d61460f6d4bbd3e3ed3b626d8198650f701c1d Mon Sep 17 00:00:00 2001 From: Louminator Date: Tue, 15 Feb 2022 23:06:23 -0500 Subject: [PATCH 02/72] A little math to modify the driving characteristics. --- .idea/modules/robot-2022.iml | 8 ++++++++ src/main/java/frc/robot/Robot.java | 23 +++++++++++++++++++++-- 2 files changed, 29 insertions(+), 2 deletions(-) create mode 100644 .idea/modules/robot-2022.iml 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/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 513fbd0..52769a6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -139,6 +139,9 @@ public class Robot extends TimedRobot { @Override public void teleopPeriodic() { double jx = joystick.getX(); double jy = -joystick.getY(); + double wheelBase = 28.0; + double leftPower; + double rightPower; //joystick deaden: yeet smol/weird joystick values when joystick is at rest double cutoff = 0.05; @@ -147,10 +150,26 @@ public class Robot extends TimedRobot { //moved this to after joystick deaden because deaden should be focused on the raw joystick values double scaleFactor = 1.0; + double R; + + if (jx >0){ + R = 10*jx*(1-jx)/((Math.pow(jx, 2)+1.0e-10)); + leftPower = -jy; + rightPower = -jy*(R - wheelBase/2)/(R + wheelBase/2); + } else{ + R = -10*jx*(1+jx)/((Math.pow(jx, 2)+1.0e-10)); + leftPower = -jy/(R + wheelBase/2)*(R - wheelBase/2); + rightPower = -jy; + } + + if (Math.abs(jx) Date: Wed, 16 Feb 2022 20:19:12 -0500 Subject: [PATCH 03/72] Renamed "Simple whatever" to "Ball whatever" and made changes to most of the code --- src/main/java/frc/robot/Robot.java | 11 +- ...oTerminal.java => BallAtoBtoTerminal.java} | 7 +- ...pleATerminal.java => BallAtoTerminal.java} | 4 +- ...pleBTerminal.java => BallBtoTerminal.java} | 2 +- .../robot/autonomous/BallCtoBtoTerminal.java | 150 ++++++++++++++++++ ...pleCTerminal.java => BallCtoTerminal.java} | 38 +++-- .../java/frc/robot/autonomous/BallSimple.java | 77 +++++++++ .../frc/robot/autonomous/BallSimpleB.java | 76 +++++++++ .../autonomous/SimpleCtoBtoTerminal.java | 4 - 9 files changed, 341 insertions(+), 28 deletions(-) rename src/main/java/frc/robot/autonomous/{SimpleAtoBtoTerminal.java => BallAtoBtoTerminal.java} (96%) rename src/main/java/frc/robot/autonomous/{SimpleATerminal.java => BallAtoTerminal.java} (98%) rename src/main/java/frc/robot/autonomous/{SimpleBTerminal.java => BallBtoTerminal.java} (98%) create mode 100644 src/main/java/frc/robot/autonomous/BallCtoBtoTerminal.java rename src/main/java/frc/robot/autonomous/{SimpleCTerminal.java => BallCtoTerminal.java} (75%) create mode 100644 src/main/java/frc/robot/autonomous/BallSimple.java create mode 100644 src/main/java/frc/robot/autonomous/BallSimpleB.java delete mode 100644 src/main/java/frc/robot/autonomous/SimpleCtoBtoTerminal.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 51cb693..e92fb6f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,14 +13,15 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.autonomous.*; import frc.robot.autonomous.GenericAutonomous; +import frc.robot.generic.Falcon; import frc.robot.generic.GenericRobot; import frc.robot.generic.Lightning; public class Robot extends TimedRobot { - GenericRobot robot = new Lightning(); + GenericRobot robot = new Falcon(); Joystick joystick = new Joystick(0); - GenericAutonomous autonomous = new autoArc(); + GenericAutonomous autonomous = new BallCtoTerminal(); int averageTurretXSize = 2; @@ -208,13 +209,13 @@ public class Robot extends TimedRobot { } if (joystick.getRawButton(5)){ - autonomous = new SimpleATerminal(); + autonomous = new BallAtoTerminal(); } if (joystick.getRawButton(6)){ - autonomous = new SimpleBTerminal(); + autonomous = new BallBtoTerminal(); } if (joystick.getRawButton(7)){ - autonomous = new SimpleCTerminal(); + autonomous = new BallCtoTerminal(); } } diff --git a/src/main/java/frc/robot/autonomous/SimpleAtoBtoTerminal.java b/src/main/java/frc/robot/autonomous/BallAtoBtoTerminal.java similarity index 96% rename from src/main/java/frc/robot/autonomous/SimpleAtoBtoTerminal.java rename to src/main/java/frc/robot/autonomous/BallAtoBtoTerminal.java index fd69f41..0d66c7e 100644 --- a/src/main/java/frc/robot/autonomous/SimpleAtoBtoTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallAtoBtoTerminal.java @@ -1,10 +1,11 @@ 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 SimpleAtoBtoTerminal extends GenericAutonomous { +public class BallAtoBtoTerminal extends GenericAutonomous { double startingYaw; double startTime; double startDistance; @@ -19,7 +20,7 @@ public class SimpleAtoBtoTerminal extends GenericAutonomous { double distanceB = 117.1; double distanceC = 160.6; double angleA = 87.74; - double angleB = 0; //NEED TO ASK CAD FOR ANGLE + double angleB = 0; //ASK CAD LATER double rampDownDist = 10; PIDController PIDDriveStraight; @@ -81,7 +82,7 @@ public void autonomousPeriodic(GenericRobot robot) { case 12: //turn to go to ball B leftpower = defaultTurnPower; rightpower = -defaultTurnPower; - //turning right + //turning left if(robot.getYaw() - startingYaw > angleA) { startingYaw = startingYaw + angleA; diff --git a/src/main/java/frc/robot/autonomous/SimpleATerminal.java b/src/main/java/frc/robot/autonomous/BallAtoTerminal.java similarity index 98% rename from src/main/java/frc/robot/autonomous/SimpleATerminal.java rename to src/main/java/frc/robot/autonomous/BallAtoTerminal.java index 06265b1..398d84c 100644 --- a/src/main/java/frc/robot/autonomous/SimpleATerminal.java +++ b/src/main/java/frc/robot/autonomous/BallAtoTerminal.java @@ -5,7 +5,7 @@ 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 BallAtoTerminal extends GenericAutonomous { double startingYaw; double leftpower; @@ -17,7 +17,7 @@ public class SimpleATerminal extends GenericAutonomous { double startTime; double startDistance; - double distanceA = 37; + double distanceA = 40.44; double distanceB = 259.26; double angleA = 87.74; diff --git a/src/main/java/frc/robot/autonomous/SimpleBTerminal.java b/src/main/java/frc/robot/autonomous/BallBtoTerminal.java similarity index 98% rename from src/main/java/frc/robot/autonomous/SimpleBTerminal.java rename to src/main/java/frc/robot/autonomous/BallBtoTerminal.java index 7dbc0b2..bc453de 100644 --- a/src/main/java/frc/robot/autonomous/SimpleBTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallBtoTerminal.java @@ -5,7 +5,7 @@ import frc.robot.generic.GenericRobot; //Simple autonomous code for ball A, closest ball to the hangar -public class SimpleBTerminal extends GenericAutonomous { +public class BallBtoTerminal extends GenericAutonomous { double startingYaw; double leftpower; double rightpower; 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..2f4e6f5 --- /dev/null +++ b/src/main/java/frc/robot/autonomous/BallCtoBtoTerminal.java @@ -0,0 +1,150 @@ +package frc.robot.autonomous; + +import edu.wpi.first.math.controller.PIDController; +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/SimpleCTerminal.java b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java similarity index 75% rename from src/main/java/frc/robot/autonomous/SimpleCTerminal.java rename to src/main/java/frc/robot/autonomous/BallCtoTerminal.java index 2005800..0f36a67 100644 --- a/src/main/java/frc/robot/autonomous/SimpleCTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java @@ -5,17 +5,22 @@ 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 BallCtoTerminal 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; + double distanceC = 40.44; + double distanceTerminal = 251; + double angleC = 84.54; + double rampDownDist = 10; + PIDController PIDDriveStraight; @Override @@ -52,16 +57,20 @@ public void autonomousPeriodic(GenericRobot robot) { leftpower = defaultPower + correction; rightpower = defaultPower - correction; - if(robot.getDriveDistanceInchesLeft() - startDistance >= 37) { + if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceC - rampDownDist){ + defaultPower = (distanceC-robot.getDriveDistanceInchesLeft()+startDistance) + *defaultPower/rampDownDist; + } + 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 @@ -75,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; @@ -88,11 +97,14 @@ public void autonomousPeriodic(GenericRobot robot) { leftpower = defaultPower + correction; rightpower = defaultPower - correction; - if(robot.getDriveDistanceInchesLeft() - startDistance >= 251) { + if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal - rampDownDist){ + defaultPower = (distanceTerminal-robot.getDriveDistanceInchesLeft()+startDistance) + *defaultPower/rampDownDist; + } + if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal){ autonomousStep += 1; - leftpower = 0; - rightpower = 0; - } //might need to tune for momentum + startTime = System.currentTimeMillis(); + } break; case 14: leftpower = 0; diff --git a/src/main/java/frc/robot/autonomous/BallSimple.java b/src/main/java/frc/robot/autonomous/BallSimple.java new file mode 100644 index 0000000..8e2eaa6 --- /dev/null +++ b/src/main/java/frc/robot/autonomous/BallSimple.java @@ -0,0 +1,77 @@ +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 C +public class BallSimple extends GenericAutonomous { + double startingYaw; + + double startDistance; + double leftpower; + double rightpower; + double defaultPower = .4; + double correction; + double startTime; + + double distance = 40.44; + //either to Balls A, B or C depending on where the robot is positioned + 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 >= distance - rampDownDist){ + defaultPower = (distance - robot.getDriveDistanceInchesLeft() + startDistance) + * defaultPower / rampDownDist; + } + if(robot.getDriveDistanceInchesLeft() - startDistance >= distance){ + 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 + } + } + + +} 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..4459ac6 --- /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; + +//Simple autonomous code for ball B +public class BallSimpleB extends GenericAutonomous { + double startingYaw; + + double startDistance; + double leftpower; + double rightpower; + double defaultPower = .4; + double correction; + double startTime; + + double distance = 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 >= distance - rampDownDist){ + defaultPower = (distance - robot.getDriveDistanceInchesLeft() + startDistance) + * defaultPower / rampDownDist; + } + if(robot.getDriveDistanceInchesLeft() - startDistance >= distance){ + 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 + } + } + + +} diff --git a/src/main/java/frc/robot/autonomous/SimpleCtoBtoTerminal.java b/src/main/java/frc/robot/autonomous/SimpleCtoBtoTerminal.java deleted file mode 100644 index 84b02d0..0000000 --- a/src/main/java/frc/robot/autonomous/SimpleCtoBtoTerminal.java +++ /dev/null @@ -1,4 +0,0 @@ -package frc.robot.autonomous; - -public class SimpleCtoBtoTerminal extends GenericAutonomous{ -} From ed65b7cb095c00dc1d5d33cd1699012fd8a0c1a6 Mon Sep 17 00:00:00 2001 From: DeathDIVer Date: Wed, 16 Feb 2022 20:30:54 -0500 Subject: [PATCH 04/72] Added robot.drivePercent(); to both simple codes --- src/main/java/frc/robot/autonomous/BallSimple.java | 1 + src/main/java/frc/robot/autonomous/BallSimpleB.java | 1 + 2 files changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/autonomous/BallSimple.java b/src/main/java/frc/robot/autonomous/BallSimple.java index 8e2eaa6..071dc46 100644 --- a/src/main/java/frc/robot/autonomous/BallSimple.java +++ b/src/main/java/frc/robot/autonomous/BallSimple.java @@ -71,6 +71,7 @@ public void autonomousPeriodic(GenericRobot robot){ break; case 6: //collect/shoot the ball again } + robot.drivePercent(leftpower, rightpower); } diff --git a/src/main/java/frc/robot/autonomous/BallSimpleB.java b/src/main/java/frc/robot/autonomous/BallSimpleB.java index 4459ac6..3105727 100644 --- a/src/main/java/frc/robot/autonomous/BallSimpleB.java +++ b/src/main/java/frc/robot/autonomous/BallSimpleB.java @@ -70,6 +70,7 @@ public void autonomousPeriodic(GenericRobot robot){ break; case 6: //collect/shoot the ball again } + robot.drivePercent(leftpower, rightpower); } From 5788dcc48686eb42804ab52a87ec0a304aba107a Mon Sep 17 00:00:00 2001 From: DeathDIVer Date: Thu, 17 Feb 2022 18:41:54 -0500 Subject: [PATCH 05/72] Made a lot of small changes: removed meaningless comments, added rampDownDist to BallBtoTerminal, and fixed a couple of naming issues --- .../robot/autonomous/BallAtoBtoTerminal.java | 12 ++++----- .../frc/robot/autonomous/BallAtoTerminal.java | 19 ++++++------- .../frc/robot/autonomous/BallBtoTerminal.java | 27 ++++++++++++++----- .../robot/autonomous/BallCtoBtoTerminal.java | 1 + .../frc/robot/autonomous/BallCtoTerminal.java | 16 ++++++++--- .../java/frc/robot/autonomous/BallSimple.java | 3 +-- .../frc/robot/autonomous/BallSimpleB.java | 4 +-- .../java/frc/robot/autonomous/autoArc.java | 1 - 8 files changed, 50 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/autonomous/BallAtoBtoTerminal.java b/src/main/java/frc/robot/autonomous/BallAtoBtoTerminal.java index 0d66c7e..3ae195f 100644 --- a/src/main/java/frc/robot/autonomous/BallAtoBtoTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallAtoBtoTerminal.java @@ -12,13 +12,13 @@ public class BallAtoBtoTerminal extends GenericAutonomous { double leftpower; double rightpower; - double defaultPower = .25; - double defaultTurnPower = .25; + double defaultPower = .4; + double defaultTurnPower = .4; double correction; double distanceA = 37; double distanceB = 117.1; - double distanceC = 160.6; + double distanceTerminal = 160.6; double angleA = 87.74; double angleB = 0; //ASK CAD LATER double rampDownDist = 10; @@ -130,10 +130,10 @@ public void autonomousPeriodic(GenericRobot robot) { leftpower = defaultPower + correction; rightpower = defaultPower - correction; - if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceC - rampDownDist){ - defaultPower = (distanceC-robot.getDriveDistanceInchesLeft()+startDistance)*defaultPower/rampDownDist; + if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal - rampDownDist){ + defaultPower = (distanceTerminal -robot.getDriveDistanceInchesLeft()+startDistance)*defaultPower/rampDownDist; } - if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceC) { + if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal) { autonomousStep += 1; leftpower = 0; rightpower = 0; diff --git a/src/main/java/frc/robot/autonomous/BallAtoTerminal.java b/src/main/java/frc/robot/autonomous/BallAtoTerminal.java index 398d84c..000a01a 100644 --- a/src/main/java/frc/robot/autonomous/BallAtoTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallAtoTerminal.java @@ -7,20 +7,18 @@ //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 = .25; - double defaultTurnPower = .25; - + double defaultPower = .4; + double defaultTurnPower = .4; double correction; - double startTime; - double startDistance; double distanceA = 40.44; - double distanceB = 259.26; + double distanceTerminal = 259.26; double angleA = 87.74; - double rampDownDist = 10; @@ -73,7 +71,6 @@ public void autonomousPeriodic(GenericRobot robot) { if (System.currentTimeMillis() - startTime > 1000){ autonomousStep = 12; } - //autonomousStep = 8; break; case 6: //collector to collect ball case 7: //collection part 2 not electric nor boogaloo @@ -100,10 +97,10 @@ public void autonomousPeriodic(GenericRobot robot) { leftpower = defaultPower + correction; rightpower = defaultPower - correction; - if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceB - rampDownDist){ - defaultPower = (distanceB-robot.getDriveDistanceInchesLeft()+startDistance)*defaultPower/rampDownDist; + if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal - rampDownDist){ + defaultPower = (distanceTerminal -robot.getDriveDistanceInchesLeft()+startDistance)*defaultPower/rampDownDist; } - if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceB) { + if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal) { autonomousStep += 1; leftpower = 0; rightpower = 0; diff --git a/src/main/java/frc/robot/autonomous/BallBtoTerminal.java b/src/main/java/frc/robot/autonomous/BallBtoTerminal.java index bc453de..7766418 100644 --- a/src/main/java/frc/robot/autonomous/BallBtoTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallBtoTerminal.java @@ -7,20 +7,25 @@ //Simple autonomous code for ball A, closest ball to the hangar public class BallBtoTerminal extends GenericAutonomous { double startingYaw; + double startDistance; + double startTime; + double leftpower; double rightpower; double defaultPower = .4; - double correction; - double startDistance; - double startTime; + + double distanceB = 61.5; + double distanceTerminal = 0; + double rampDownDist = 10; + PIDController PIDDriveStraight; @Override public void autonomousInit(GenericRobot robot) { autonomousStep = 0; - startingYaw = robot.getYaw(); //might need to change to set degrees + startingYaw = robot.getYaw(); PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD()); startTime = System.currentTimeMillis(); } @@ -50,10 +55,13 @@ public void autonomousPeriodic(GenericRobot robot) { leftpower = defaultPower + correction; rightpower = defaultPower - correction; - if(robot.getDriveDistanceInchesLeft() >= 61.5){ + if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceB - rampDownDist){ + defaultPower = (distanceB-robot.getDriveDistanceInchesLeft()+startDistance)*defaultPower/rampDownDist; + } + if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceB){ autonomousStep += 1; startTime = System.currentTimeMillis(); - } //has 3 inches of momentum with .25 power + } break; case 5: //stop leftpower = 0; @@ -81,8 +89,13 @@ public void autonomousPeriodic(GenericRobot robot) { leftpower = defaultPower + correction; rightpower = defaultPower - correction; - if(robot.getDriveDistanceInchesLeft() - startDistance >= 153.5){ + 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 14: diff --git a/src/main/java/frc/robot/autonomous/BallCtoBtoTerminal.java b/src/main/java/frc/robot/autonomous/BallCtoBtoTerminal.java index 2f4e6f5..4603dcc 100644 --- a/src/main/java/frc/robot/autonomous/BallCtoBtoTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallCtoBtoTerminal.java @@ -1,6 +1,7 @@ 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 diff --git a/src/main/java/frc/robot/autonomous/BallCtoTerminal.java b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java index 0f36a67..83f6dc2 100644 --- a/src/main/java/frc/robot/autonomous/BallCtoTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java @@ -26,7 +26,7 @@ public class BallCtoTerminal extends GenericAutonomous { @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()); @@ -79,7 +79,15 @@ public void autonomousPeriodic(GenericRobot robot) { 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 @ terminal + case 12://reset + if (System.currentTimeMillis() - startTime >= 1000){ + PIDDriveStraight.reset(); + PIDDriveStraight.enableContinuousInput(-180,180); + startDistance = robot.getDriveDistanceInchesLeft(); + autonomousStep +=1; + } + break; + case 13: //turn to go to ball @ terminal leftpower = -defaultTurnPower; rightpower = defaultTurnPower; //turning left @@ -91,7 +99,7 @@ public void autonomousPeriodic(GenericRobot robot) { autonomousStep += 1; } break; - case 13: //drive towards the ball + case 14: //drive towards the ball correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); leftpower = defaultPower + correction; @@ -106,7 +114,7 @@ public void autonomousPeriodic(GenericRobot robot) { startTime = System.currentTimeMillis(); } break; - case 14: + case 15: leftpower = 0; rightpower = 0; break; diff --git a/src/main/java/frc/robot/autonomous/BallSimple.java b/src/main/java/frc/robot/autonomous/BallSimple.java index 071dc46..bd5604f 100644 --- a/src/main/java/frc/robot/autonomous/BallSimple.java +++ b/src/main/java/frc/robot/autonomous/BallSimple.java @@ -15,8 +15,7 @@ public class BallSimple extends GenericAutonomous { double correction; double startTime; - double distance = 40.44; - //either to Balls A, B or C depending on where the robot is positioned + double distance = 40.44; //either to Balls A, B or C depending on where the robot is positioned double rampDownDist = 10; PIDController PIDDriveStraight; diff --git a/src/main/java/frc/robot/autonomous/BallSimpleB.java b/src/main/java/frc/robot/autonomous/BallSimpleB.java index 3105727..04f16b3 100644 --- a/src/main/java/frc/robot/autonomous/BallSimpleB.java +++ b/src/main/java/frc/robot/autonomous/BallSimpleB.java @@ -1,6 +1,7 @@ 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 @@ -14,8 +15,7 @@ public class BallSimpleB extends GenericAutonomous { double correction; double startTime; - double distance = 61.5; - //distance to ball B from the tarmac + double distance = 61.5; //distance to ball B from the tarmac double rampDownDist = 10; PIDController PIDDriveStraight; diff --git a/src/main/java/frc/robot/autonomous/autoArc.java b/src/main/java/frc/robot/autonomous/autoArc.java index 2c94ef8..31e10f8 100644 --- a/src/main/java/frc/robot/autonomous/autoArc.java +++ b/src/main/java/frc/robot/autonomous/autoArc.java @@ -1,6 +1,5 @@ package frc.robot.autonomous; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.generic.GenericRobot; From f3a8c8483b95aa8a91f7805736109ca055c33648 Mon Sep 17 00:00:00 2001 From: DeathDIVer Date: Thu, 17 Feb 2022 18:57:22 -0500 Subject: [PATCH 06/72] Committing the code to make sure its here, most of the values are still off --- .../java/frc/robot/autonomous/BallAtoB.java | 120 ++++++++++++ .../java/frc/robot/autonomous/BallBtoA.java | 100 ++++++++++ .../java/frc/robot/autonomous/BallBtoC.java | 100 ++++++++++ .../java/frc/robot/autonomous/BallCtoB.java | 116 +++++++++++ .../java/frc/robot/autonomous/autoArc.java | 1 + .../robot/autonomous/autoArcDifferent.java | 181 ++++++++++++++++++ 6 files changed, 618 insertions(+) create mode 100644 src/main/java/frc/robot/autonomous/BallAtoB.java create mode 100644 src/main/java/frc/robot/autonomous/BallBtoA.java create mode 100644 src/main/java/frc/robot/autonomous/BallBtoC.java create mode 100644 src/main/java/frc/robot/autonomous/BallCtoB.java create mode 100644 src/main/java/frc/robot/autonomous/autoArcDifferent.java diff --git a/src/main/java/frc/robot/autonomous/BallAtoB.java b/src/main/java/frc/robot/autonomous/BallAtoB.java new file mode 100644 index 0000000..7b28128 --- /dev/null +++ b/src/main/java/frc/robot/autonomous/BallAtoB.java @@ -0,0 +1,120 @@ +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 BallAtoB extends GenericAutonomous { + double startingYaw; + + double leftpower; + double rightpower; + double defaultPower = .25; + double defaultTurnPower = .25; + + double correction; + double startTime; + double startDistance; + + double distanceA = 40.44; + double distanceB = 259.26; //CHANGE + double angleA = 87.74; + + 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(); + 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 >= 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; + } + //autonomousStep = 8; + 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 @ terminal + 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 + 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: + leftpower = 0; + rightpower = 0; + break; + } + robot.drivePercent(leftpower, rightpower); + + } +} diff --git a/src/main/java/frc/robot/autonomous/BallBtoA.java b/src/main/java/frc/robot/autonomous/BallBtoA.java new file mode 100644 index 0000000..6a0c475 --- /dev/null +++ b/src/main/java/frc/robot/autonomous/BallBtoA.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 BallBtoA 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/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/BallCtoB.java b/src/main/java/frc/robot/autonomous/BallCtoB.java new file mode 100644 index 0000000..78b532e --- /dev/null +++ b/src/main/java/frc/robot/autonomous/BallCtoB.java @@ -0,0 +1,116 @@ +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, closest ball to the hangar, and driving to the ball at terminal +public class BallCtoB extends GenericAutonomous { + double startingYaw; + + double startDistance; + double leftpower; + double rightpower; + double defaultPower = .4; + double defaultTurnPower = .4; + double correction; + double startTime; + + double distanceC = 40.44; + double distanceB = 0; + double angleC = 84.54; + 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){ + defaultPower = (distanceC-robot.getDriveDistanceInchesLeft()+startDistance) + *defaultPower/rampDownDist; + } + 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: //collector to collect ball + case 7: //collection part 2 not electric nor boogaloo + case 8: //another 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 @ terminal + leftpower = -defaultTurnPower; + rightpower = defaultTurnPower; + //turning left + + if(robot.getYaw() - startingYaw < -angleC) { + startingYaw = startingYaw - angleC; + startDistance = robot.getDriveDistanceInchesLeft(); + PIDDriveStraight.reset(); + autonomousStep += 1; + } + break; + case 13: //drive towards the ball + 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 14: + leftpower = 0; + rightpower = 0; + break; + } + robot.drivePercent(leftpower, rightpower); + } +} diff --git a/src/main/java/frc/robot/autonomous/autoArc.java b/src/main/java/frc/robot/autonomous/autoArc.java index 31e10f8..643ba66 100644 --- a/src/main/java/frc/robot/autonomous/autoArc.java +++ b/src/main/java/frc/robot/autonomous/autoArc.java @@ -4,6 +4,7 @@ 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; 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..f5057b2 --- /dev/null +++ b/src/main/java/frc/robot/autonomous/autoArcDifferent.java @@ -0,0 +1,181 @@ +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); + + } + +} From 34e0cd01345edeeebbd6840a68c999bfba8ebb94 Mon Sep 17 00:00:00 2001 From: DeathDIVer Date: Thu, 17 Feb 2022 19:03:27 -0500 Subject: [PATCH 07/72] yes --- src/main/java/frc/robot/autonomous/BallSimple.java | 2 -- src/main/java/frc/robot/autonomous/autoArcDifferent.java | 2 ++ 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/autonomous/BallSimple.java b/src/main/java/frc/robot/autonomous/BallSimple.java index bd5604f..3ed5203 100644 --- a/src/main/java/frc/robot/autonomous/BallSimple.java +++ b/src/main/java/frc/robot/autonomous/BallSimple.java @@ -72,6 +72,4 @@ public void autonomousPeriodic(GenericRobot robot){ } robot.drivePercent(leftpower, rightpower); } - - } diff --git a/src/main/java/frc/robot/autonomous/autoArcDifferent.java b/src/main/java/frc/robot/autonomous/autoArcDifferent.java index f5057b2..320fcc0 100644 --- a/src/main/java/frc/robot/autonomous/autoArcDifferent.java +++ b/src/main/java/frc/robot/autonomous/autoArcDifferent.java @@ -176,6 +176,8 @@ public void autonomousPeriodic(GenericRobot robot) { robot.drivePercent(leftPower, rightPower); robot.setTurretPowerPct(currentTurretPower); + + } } From acc839d663c74e6bcadafae7529e2d80f9826c0e Mon Sep 17 00:00:00 2001 From: DeathDIVer Date: Thu, 17 Feb 2022 19:17:44 -0500 Subject: [PATCH 08/72] added turret tracking --- .../frc/robot/autonomous/BallAtoTerminal.java | 6 +- .../frc/robot/autonomous/BallCtoTerminal.java | 7 ++- .../frc/robot/autonomous/TurretTracker.java | 59 +++++++++++++++++++ 3 files changed, 70 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/autonomous/TurretTracker.java diff --git a/src/main/java/frc/robot/autonomous/BallAtoTerminal.java b/src/main/java/frc/robot/autonomous/BallAtoTerminal.java index 000a01a..247a7db 100644 --- a/src/main/java/frc/robot/autonomous/BallAtoTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallAtoTerminal.java @@ -21,19 +21,22 @@ public class BallAtoTerminal extends GenericAutonomous { double angleA = 87.74; double rampDownDist = 10; - PIDController PIDDriveStraight; + TurretTracker tracker = new TurretTracker(); + @Override public void autonomousInit(GenericRobot robot) { autonomousStep = 0; startingYaw = robot.getYaw(); startTime = System.currentTimeMillis(); PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDpivotD()); + tracker.turretInit(robot); } @Override public void autonomousPeriodic(GenericRobot robot) { + tracker.turretUpdate(robot); switch(autonomousStep){ case 0: //reset @@ -112,6 +115,7 @@ public void autonomousPeriodic(GenericRobot robot) { break; } robot.drivePercent(leftpower, rightpower); + tracker.turretMove(robot); } } diff --git a/src/main/java/frc/robot/autonomous/BallCtoTerminal.java b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java index 83f6dc2..6e0b890 100644 --- a/src/main/java/frc/robot/autonomous/BallCtoTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java @@ -23,17 +23,20 @@ public class BallCtoTerminal extends GenericAutonomous { PIDController PIDDriveStraight; + TurretTracker tracker = new TurretTracker(); + @Override public void autonomousInit(GenericRobot robot) { autonomousStep = 0; startingYaw = robot.getYaw(); startTime = System.currentTimeMillis(); PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD()); - + tracker.turretInit(robot); } @Override public void autonomousPeriodic(GenericRobot robot) { + tracker.turretUpdate(robot); switch(autonomousStep){ case 0: //reset @@ -120,5 +123,7 @@ public void autonomousPeriodic(GenericRobot robot) { break; } robot.drivePercent(leftpower, rightpower); + tracker.turretMove(robot); + } } 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); + } +} From cd845835499c2573ac273d414105e34b8680f230 Mon Sep 17 00:00:00 2001 From: Malti Date: Thu, 17 Feb 2022 20:17:19 -0500 Subject: [PATCH 09/72] Added in Hangar Climb. Logic completely written, but need to find many values still. --- src/main/java/frc/robot/Robot.java | 58 ++-- src/main/java/frc/robot/command/Hang.java | 259 ++++++++++++++++++ .../java/frc/robot/generic/GenericRobot.java | 28 ++ .../java/frc/robot/generic/Lightning.java | 64 +++++ 4 files changed, 393 insertions(+), 16 deletions(-) create mode 100644 src/main/java/frc/robot/command/Hang.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b78da79..5be0e75 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,6 +13,7 @@ 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; @@ -22,6 +23,7 @@ public class Robot extends TimedRobot { GenericRobot robot = new TurretBot(); Joystick joystick = new Joystick(0); GenericAutonomous autonomous = new SimpleBTerminal(); + GenericCommand command = new Hang(); int averageTurretXSize = 2; @@ -34,6 +36,9 @@ public class Robot extends TimedRobot { int counter = 0; double average; double currentTurretPower; + boolean hang = false; + int count = 0; + boolean reset = true; PIDController turretPIDController; @@ -125,7 +130,8 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { turretPIDController = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD()); - + hang = false; + count = 0; } @Override public void teleopPeriodic() { @@ -146,26 +152,46 @@ public class Robot extends TimedRobot { ); //note to self: buttons currently assume mirrored joystick setting - if (joystick.getRawButton(11)) robot.setCollectorIntakePercentage( 1.0); - else if (joystick.getRawButton(16)) robot.setCollectorIntakePercentage(-1.0); - else robot.setCollectorIntakePercentage( 0); + if (joystick.getRawButtonPressed(2)){ + count = (count+1)%2; + } + if (count == 1){ + hang = true; + } + else{ + hang = false; + } - if (joystick.getRawButton(12)) robot.setTurretPowerPct( 0.2); - else if (joystick.getRawButton(15)) robot.setTurretPowerPct(-0.2); - else robot.setTurretPowerPct( 0.0); + if (!hang) { + reset = true; + if (joystick.getRawButton(11)) robot.setCollectorIntakePercentage(1.0); + else if (joystick.getRawButton(16)) robot.setCollectorIntakePercentage(-1.0); + else robot.setCollectorIntakePercentage(0); - if (joystick.getRawButton(13)) robot.setShooterPowerPct( 0.2, 0.2); - else if (joystick.getRawButton(14)) robot.setShooterPowerPct(-0.2, -0.2); - else robot.setShooterPowerPct( 0.0, 0.0); + if (joystick.getRawButton(12)) robot.setTurretPowerPct(0.2); + else if (joystick.getRawButton(15)) robot.setTurretPowerPct(-0.2); + else robot.setTurretPowerPct(0.0); - if (joystick.getRawButton( 7)) robot.raiseCollector(); - if (joystick.getRawButton( 8)) robot.lowerCollector(); + if (joystick.getRawButton(13)) robot.setShooterPowerPct(0.2, 0.2); + else if (joystick.getRawButton(14)) robot.setShooterPowerPct(-0.2, -0.2); + else robot.setShooterPowerPct(0.0, 0.0); - if (joystick.getRawButton( 6)) robot.turnOnPTO(); - if (joystick.getRawButton( 9)) robot.turnOffPTO(); + if (joystick.getRawButton(7)) robot.raiseCollector(); + if (joystick.getRawButton(8)) robot.lowerCollector(); - if (joystick.getRawButton( 5)) robot.setArmsForward(); - if (joystick.getRawButton(10)) robot.setArmsBackward(); + if (joystick.getRawButton(6)) robot.turnOnPTO(); + if (joystick.getRawButton(9)) robot.turnOffPTO(); + + if (joystick.getRawButton(5)) robot.setArmsForward(); + if (joystick.getRawButton(10)) robot.setArmsBackward(); + } + else{ + if (reset){ + command.begin(robot); + reset = false; + } + command.step(robot); + } //Start of Daniel+Saiarun Turret test 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..360dee2 --- /dev/null +++ b/src/main/java/frc/robot/command/Hang.java @@ -0,0 +1,259 @@ +package frc.robot.command; + +import edu.wpi.first.math.controller.PIDController; +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 = 12.0; + double Tapetheta = 0; + + double correction; + double currentYaw; + + long startingTime = 0; + + boolean leftSensor = false; + boolean rightSensor = false; + + double outerDistArc; + double lTraveled; + + double fwd = 48; + PIDController PIDSteering; + int commandStep = -1; + boolean tapeAlign; + + /////^^^^^^^^^^^Stuff for tapeAlign + + + //////////////Now the real stuff + double desiredHeight; + double lowHeight; + double escapeHeight; + double getToBarHeight; + boolean firstTime = true; + + + public void begin(GenericRobot robot){ + startingTime = System.currentTimeMillis(); + commandStep = -1; + leftSensor = false; + rightSensor = false; + lTraveled = 0; + fwd = 48; + PIDSteering = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD()); + tapeAlign = true; + firstTime = true; + } + + public void step(GenericRobot robot){ + if (tapeAlign) { + switch (commandStep) { /////////////tapeAlign Code + case -1: + robot.resetEncoders(); + robot.resetAttitude(); + + if (System.currentTimeMillis() >= startingTime + 100) { + commandStep += 1; + } + break; + case 0: + startAngle = robot.getYaw(); + PIDSteering.reset(); + PIDSteering.enableContinuousInput(-180, 180); + commandStep += 1; + break; + case 1: + correction = PIDSteering.calculate(robot.getYaw() - startAngle); + leftPower = defaultPower + correction; //didn't we stop doing this? + rightPower = defaultPower - correction; + + if (!robot.getTapeSensorOne()) { + startDistance = robot.getDriveDistanceInchesLeft(); + leftSensor = true; + commandStep += 1; + } else if (!robot.getTapeSensorTwo()) { + 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.getTapeSensorTwo()) { + differenceDistance = Math.abs(robot.getDriveDistanceInchesLeft() - startDistance); + Tapetheta = Math.atan(differenceDistance / sensorDist) * 180 / Math.PI; + outerDistArc = robot.getDriveDistanceInchesRight(); + commandStep += 1; + } else if (!leftSensor && !robot.getTapeSensorOne()) { + differenceDistance = Math.abs(robot.getDriveDistanceInchesLeft() - startDistance); + Tapetheta = Math.atan(differenceDistance / sensorDist) * 180 / Math.PI; + outerDistArc = robot.getDriveDistanceInchesLeft(); + commandStep = 4; + } + break; + case 3://///////////////////////////////////////////////////////////////////skip this step + if (leftSensor) { + leftPower = defaultPower * .5; + rightPower = defaultPower * 2; + } else { + rightPower = defaultPower * .5; + leftPower = defaultPower * 2; + } + currentYaw = robot.getYaw(); + if (Math.abs(Math.signum(currentYaw - startAngle) * (((Math.abs(currentYaw - startAngle) + 180) % 360) - 180)) >= Math.abs(Tapetheta)) { + if (rightSensor) { + outerDistArc = robot.getDriveDistanceInchesLeft() - outerDistArc; + } else { + outerDistArc = robot.getDriveDistanceInchesRight() - outerDistArc; + } + lTraveled = Math.abs(outerDistArc / (Tapetheta * Math.PI / 180) * Math.sin(Math.abs(Tapetheta * Math.PI / 180))); + commandStep += 1; + }///////////////////////////////////////this step is skipped + break; + case 4: + 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 5: + correction = PIDSteering.calculate(robot.getYaw() - currentYaw); + leftPower = defaultPower + correction; + rightPower = defaultPower - correction; + if (Math.abs(robot.getDriveDistanceInchesLeft() - startDistance) >= (fwd - lTraveled)) { + leftPower = 0; + rightPower = 0; + commandStep += 1; + } + break; + case 6: //adios amigos + leftPower = 0; + rightPower = 0; + tapeAlign = false; + commandStep = 0; + break; + } + robot.drivePercent(leftPower, rightPower); + } + else{////////////////////////////start the real stuff now + switch (commandStep){ + case 0:///reset and enable PTO + robot.turnOnPTO(); + commandStep += 1; + break; + case 1: //////raise climber arms (skip 10 steps after in case we need to scoot/scoot + if (robot.armHeight() >= desiredHeight){ + robot.armPower(0); + commandStep += 1; + } + else{ + robot.raiseClimberArms(); + } + break; + case 2: //////disable PTO + robot.turnOffPTO(); + commandStep += 1; + break; + case 3: //////scoot back a lil + //go back, if in the right place, next step + if (robot.inTheRightPlace()){ + commandStep += 1; + robot.armPower(0); // because this is really the drivetrain, it will work + } + else{ + robot.lowerClimberArms(); //because this is really the drivetrain, it will work + } + break; + case 4: //////enable PTO + robot.turnOnPTO(); + commandStep = 11; + break; + case 11: ////////lower climber arms + if (robot.armInContact() && robot.armHeight() <= lowHeight){ + robot.armPower(0); + commandStep += 1; + } + else{ + robot.lowerClimberArms(); + } + break; + case 12: /////////////raise arms slightly + if (robot.armHeight() >= escapeHeight && !robot.armInContact()){ + robot.armPower(0); + commandStep += 1; + } + else{ + robot.raiseClimberArms(); + } + break; + case 13: ///////////unlock rotation piston to send arms forward + robot.setArmsForward(); + commandStep += 1; + break; + case 14: ///////////move arms forward + if (robot.armHeight() >= getToBarHeight){ + robot.armPower(0); + commandStep += 1; + } + else{ + robot.raiseClimberArms(); + } + break; + case 15: + if (robot.armInContact()){ + commandStep += 1; + robot.armPower(0); + } + else{ + robot.lowerClimberArms(); + } + case 16://///////once in contact move arms back again with the piston and swiiiiing + robot.setArmsBackward(); + commandStep += 1; + break; + case 17://////////go back to case 11 and repeat down to this step + if (firstTime){ + commandStep = 11; + firstTime = false; + } + else{ + commandStep += 1; + robot.armPower(0); + } + break; + case 19:///////lift all the way up to be extra secure + if (robot.armHeight() <= lowHeight && robot.armInContact()){ + robot.armPower(0); + commandStep += 1; + } + else{ + robot.lowerClimberArms(); + } + case 18: ////////now we are done. If all goes well, we are on the traversal rung, if not, we no longer have a robot >;( + robot.armPower(0); + break; + + } + } + } +} diff --git a/src/main/java/frc/robot/generic/GenericRobot.java b/src/main/java/frc/robot/generic/GenericRobot.java index 900a994..6f36e9c 100644 --- a/src/main/java/frc/robot/generic/GenericRobot.java +++ b/src/main/java/frc/robot/generic/GenericRobot.java @@ -108,6 +108,10 @@ public default void resetAttitude() { System.out.println("I don't have a navx"); } + boolean getTapeSensorOne(); + + boolean getTapeSensorTwo(); + public enum TeamColor { RED, BLUE, @@ -282,4 +286,28 @@ public default void setArmsForward() { public default void setArmsBackward() { //System.out.println("I don't have a climber"); } + public default void raiseClimberArms(){ + //System.out.println("I don't have a climber"); + } + public default void lowerClimberArms(){ + //System.out.println("I don't have a climber"); + } + public default void armPower(double power){ + //System.out.println("Elbow Grease"); + } + public default double armHeight(){ + return 0.0; + //System.out.println("I don't have a climber"); + } + + 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"); + } + } diff --git a/src/main/java/frc/robot/generic/Lightning.java b/src/main/java/frc/robot/generic/Lightning.java index 3869702..97ce6d3 100644 --- a/src/main/java/frc/robot/generic/Lightning.java +++ b/src/main/java/frc/robot/generic/Lightning.java @@ -14,6 +14,11 @@ public class Lightning implements GenericRobot { public static final double TICKS_PER_REVOLUTION_SHOOTERA = 116; public static final double TICKS_PER_REVOLUTION_SHOOTERB = 116; + 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); CANSparkMax collector = new CANSparkMax( 3, kBrushless); @@ -292,6 +297,55 @@ public void setArmsBackward(){ arms.set(DoubleSolenoid.Value.kForward); } + @Override + public void raiseClimberArms() { + leftMotorB.set(.1); + leftMotorA.set(.1); + rightMotorA.set(.1); + rightMotorB.set(.1); + } + + @Override + public void lowerClimberArms() { + leftMotorB.set(-.1); + leftMotorA.set(-.1); + rightMotorA.set(-.1); + rightMotorB.set(-.1); + } + + @Override + public void armPower(double power) { + leftMotorB.set(power); + leftMotorA.set(power); + rightMotorA.set(power); + rightMotorB.set(power); + } + + @Override + public double armHeight() { + //TODO + //Maybe use some sensor. Do NOT want to use encoders for this. + return GenericRobot.super.armHeight(); + } + + @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; + } + } + + @Override + public boolean inTheRightPlace(){ + //TODO: maybe a sensor?? + return false; + } + @Override public double getPIDmaneuverP() { return GenericRobot.super.getPIDmaneuverP(); @@ -335,6 +389,16 @@ public void resetAttitude() { navx.reset(); } + @Override + public boolean getTapeSensorOne() { + return false; + } + + @Override + public boolean getTapeSensorTwo() { + return false; + } + @Override public double turretPIDgetP() { return GenericRobot.super.turretPIDgetP(); From b13c45fa4ddf875d2b9fddcb36aeea60fcd383bc Mon Sep 17 00:00:00 2001 From: DeathDIVer Date: Sun, 20 Feb 2022 12:50:46 -0500 Subject: [PATCH 10/72] updated robot.java --- src/main/java/frc/robot/Robot.java | 16 ++++++++-------- .../frc/robot/autonomous/BallCtoTerminal.java | 1 - 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3513d2f..cd1ef8c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -23,9 +23,9 @@ public class Robot extends TimedRobot { //Add new Autos here when they're authored public static final GenericAutonomous autoArc = new autoArc(), - simpleATerminal = new SimpleATerminal(), - simpleBTerminal = new SimpleBTerminal(), - simpleCTerminal = new SimpleCTerminal(); + simpleATerminal = new BallAtoTerminal(), + simpleBTerminal = new BallBtoTerminal(), + simpleCTerminal = new BallCtoTerminal(); GenericRobot robot = new TurretBot(); Joystick joystick = new Joystick(0); @@ -246,8 +246,8 @@ else if(xbox.getRawAxis(rightAxis) < -tolerance){ //Collector indexer logic based on cargo already in sensors (from jack) double defCollectorPower = 1; double defIndexerPower = 1; - double curCollector = 0; - double curIndexer = 0; + double curCollector; + double curIndexer; //button 2 = bottom center button if(joystick.getRawButton(2)){ @@ -293,9 +293,9 @@ else if (joystick.getRawButton(5)){ } if (joystick.getRawButton(4)) autonomous = autoArc; - if (joystick.getRawButton(5)) autonomous = BallAtoTerminal; - if (joystick.getRawButton(6)) autonomous = BallBtoTerminal; - if (joystick.getRawButton(7)) autonomous = BallCtoTerminal; + if (joystick.getRawButton(5)) autonomous = simpleATerminal; + if (joystick.getRawButton(6)) autonomous = simpleBTerminal; + if (joystick.getRawButton(7)) autonomous = simpleCTerminal; } diff --git a/src/main/java/frc/robot/autonomous/BallCtoTerminal.java b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java index 6e0b890..986b993 100644 --- a/src/main/java/frc/robot/autonomous/BallCtoTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java @@ -124,6 +124,5 @@ public void autonomousPeriodic(GenericRobot robot) { } robot.drivePercent(leftpower, rightpower); tracker.turretMove(robot); - } } From f0460ff031591f9c1476404e2f75e97eac464916 Mon Sep 17 00:00:00 2001 From: DeathDIVer Date: Sun, 20 Feb 2022 13:21:12 -0500 Subject: [PATCH 11/72] updated robot.java --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cd1ef8c..92fa969 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -30,7 +30,7 @@ public class Robot extends TimedRobot { GenericRobot robot = new TurretBot(); Joystick joystick = new Joystick(0); Joystick xbox = new Joystick(1); - GenericAutonomous autonomous = autoArc; + GenericAutonomous autonomous = simpleCTerminal; int averageTurretXSize = 2; From 03f15ead5411211ef6a613da13d2fa80cf78dc75 Mon Sep 17 00:00:00 2001 From: JW314 Date: Sun, 20 Feb 2022 13:45:49 -0500 Subject: [PATCH 12/72] Implemented code to shoot a cargo (startup, rpm check, indexer push) --- src/main/java/frc/robot/Robot.java | 21 +++++++++++++++++++ .../java/frc/robot/generic/GenericRobot.java | 8 +++++-- .../java/frc/robot/generic/Lightning.java | 5 ++++- 3 files changed, 31 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f016cfc..44cedb7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -242,6 +242,27 @@ else if(xbox.getRawAxis(rightAxis) < -tolerance){ robot.setTurretPowerPct(currentTurretPower); + double shooterTargetRPM = 0; + if (joystick.getRawButton(10)){ + shooterTargetRPM = robot.getShooterTargetRPM(); + }else{ + shooterTargetRPM = 0; + } + + robot.setShooterRPM(shooterTargetRPM, shooterTargetRPM); + + if(joystick.getRawButton(16)){ + if(robot.isReadyToShoot()){ + robot.setActivelyShooting(true); + } + else{ + robot.setActivelyShooting(false); + } + } + else{ + robot.setActivelyShooting(false); + } + //Collector indexer logic based on cargo already in sensors (from jack) double defCollectorPower = 1; diff --git a/src/main/java/frc/robot/generic/GenericRobot.java b/src/main/java/frc/robot/generic/GenericRobot.java index 4139de6..c1394aa 100644 --- a/src/main/java/frc/robot/generic/GenericRobot.java +++ b/src/main/java/frc/robot/generic/GenericRobot.java @@ -338,8 +338,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(){ diff --git a/src/main/java/frc/robot/generic/Lightning.java b/src/main/java/frc/robot/generic/Lightning.java index fe79b42..e8c55fa 100644 --- a/src/main/java/frc/robot/generic/Lightning.java +++ b/src/main/java/frc/robot/generic/Lightning.java @@ -54,7 +54,10 @@ public class Lightning implements GenericRobot { SparkMaxLimitSwitch limitSwitchLeftBForward = leftMotorB.getForwardLimitSwitch(lstype); SparkMaxLimitSwitch limitSwitchLeftBReverse = leftMotorB.getReverseLimitSwitch(lstype); + double defaultShooterTargetRPM = 500; + boolean isPTOonArms; + //True = robot is in the process of and committed to shooting a cargo at mach 12 boolean isActivelyShooting; @@ -325,7 +328,7 @@ public void setShooterTargetDistance(double length, double height) { @Override public double getShooterTargetRPM(){ - return 1000; + return defaultShooterTargetRPM; } @Override From deff344da916d69d85d544c4c8137233aafec2c2 Mon Sep 17 00:00:00 2001 From: Louminator Date: Sun, 20 Feb 2022 13:50:47 -0500 Subject: [PATCH 13/72] Set up new double solenoid --- .gitignore | 4 +++- .idea/misc.xml | 2 +- src/main/java/frc/robot/generic/Lightning.java | 6 +++--- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/.gitignore b/.gitignore index 9de817b..24eba5d 100644 --- a/.gitignore +++ b/.gitignore @@ -238,4 +238,6 @@ fabric.properties .idea/httpRequests # Android studio 3.1+ serialized cache file -.idea/caches/build_file_checksums.ser \ No newline at end of file +.idea/caches/build_file_checksums.ser + +.idea/** 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/src/main/java/frc/robot/generic/Lightning.java b/src/main/java/frc/robot/generic/Lightning.java index fe79b42..b1a4dfe 100644 --- a/src/main/java/frc/robot/generic/Lightning.java +++ b/src/main/java/frc/robot/generic/Lightning.java @@ -38,7 +38,7 @@ public class Lightning implements GenericRobot { //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(); @@ -341,12 +341,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 From c4b8c36d52b44f301573e0485661f1cd5111e342 Mon Sep 17 00:00:00 2001 From: Malti Date: Sun, 20 Feb 2022 17:01:14 -0500 Subject: [PATCH 14/72] merged in main and climber code. needs testing --- src/main/java/frc/robot/Robot.java | 134 ++++++--- src/main/java/frc/robot/command/Hang.java | 279 ++++++++++++++---- .../java/frc/robot/generic/GenericRobot.java | 27 +- .../java/frc/robot/generic/Lightning.java | 73 ++--- 4 files changed, 380 insertions(+), 133 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 94362fb..4be7e0e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -48,6 +48,10 @@ public class Robot extends TimedRobot { boolean hang = false; int count = 0; boolean reset = true; + double maxCurrentLeftA = 0; + double maxCurrentLeftB = 0; + double maxCurrentRightA = 0; + double maxCurrentRightB = 0; PIDController turretPIDController; @@ -58,6 +62,19 @@ public class Robot extends TimedRobot { @Override public void robotPeriodic() { + 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(); + } + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); NetworkTableEntry tx = table.getEntry("tx"); NetworkTableEntry ty = table.getEntry("ty"); @@ -70,6 +87,11 @@ public class Robot extends TimedRobot { turretarea = ta.getDouble(0.0); turretv = tv.getDouble(0.0); + SmartDashboard.putNumber("LeftACurrentMax", maxCurrentLeftA); + SmartDashboard.putNumber("LeftBCurrentMax", maxCurrentLeftB); + SmartDashboard.putNumber("RightACurrentMax", maxCurrentRightA); + SmartDashboard.putNumber("RightBCurrentMax", maxCurrentRightB); + SmartDashboard.putNumber("tv", turretv); SmartDashboard.putNumber("LimelightX", turretx); @@ -146,6 +168,14 @@ public class Robot extends TimedRobot { SmartDashboard.putNumber("Autonomous Step", autonomous.autonomousStep); + SmartDashboard.putNumber("leftEncoderRaw", robot.encoderTicksLeftDrive()); + SmartDashboard.putNumber("rightEncoderRaw", robot.encoderTicksRightDrive()); + SmartDashboard.putBoolean("leftTapeSensor", robot.getFloorSensorLeft()); + SmartDashboard.putBoolean("rightTapeSensor", robot.getFloorSensorRight()); + SmartDashboard.putBoolean("leftCLimberSensor", robot.getClimbSensorLeft()); + SmartDashboard.putBoolean("rightClimberSensor", robot.getClimbSensorRight()); + + } @Override public void autonomousInit() { @@ -163,39 +193,9 @@ public class Robot extends TimedRobot { } @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; - - //moved this to after joystick deaden because deaden should be focused on the raw joystick values - double scaleFactor = 1.0; - - //robot PTO not on arms, give joystick carte blanche - if(!robot.getPTOState()){ - robot.drivePercent( - (jy+jx) * scaleFactor, - (jy-jx) * scaleFactor - ); - } - - - 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)); - - - - //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; //note to self: buttons currently assume mirrored joystick setting - if (joystick.getRawButtonPressed(2)){ + if (joystick.getRawButtonPressed(6)){ count = (count+1)%2; } if (count == 1){ @@ -207,6 +207,39 @@ public class Robot extends TimedRobot { if (!hang) { reset = true; + + 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; + + //moved this to after joystick deaden because deaden should be focused on the raw joystick values + double scaleFactor = 1.0; + + //robot PTO not on arms, give joystick carte blanche + if(!robot.getPTOState()){ + robot.drivePercent( + (jy+jx) * scaleFactor, + (jy-jx) * scaleFactor + ); + } + + + 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)); + + + + //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); @@ -230,6 +263,7 @@ else if(xbox.getRawAxis(rightAxis) < -tolerance){ } robot.drivePercent(driveLeft, driveRight); } + } else{ if (reset){ @@ -350,10 +384,40 @@ else if (xbox.getRawButton(2)){ //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)) { diff --git a/src/main/java/frc/robot/command/Hang.java b/src/main/java/frc/robot/command/Hang.java index 360dee2..5930330 100644 --- a/src/main/java/frc/robot/command/Hang.java +++ b/src/main/java/frc/robot/command/Hang.java @@ -1,6 +1,7 @@ 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{ @@ -40,9 +41,18 @@ public class Hang extends GenericCommand{ //////////////Now the real stuff double desiredHeight; double lowHeight; - double escapeHeight; + double escapeHeight = 10;///TODO:what is this?? double getToBarHeight; boolean firstTime = true; + int countLeft = 0; + int countRight = 0; + double leftArmPower = 0; + double rightArmPower = 0; + double defaultClimbPower = .1; + boolean leftArrived = false; + boolean rightArrived = false; + double startHeightLeft = 0; + double startHeightRight = 0; public void begin(GenericRobot robot){ @@ -51,13 +61,22 @@ public void begin(GenericRobot robot){ leftSensor = false; rightSensor = false; lTraveled = 0; - fwd = 48; + fwd = 75.6; PIDSteering = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD()); tapeAlign = true; firstTime = true; } public void step(GenericRobot robot){ + SmartDashboard.putNumber("leftEncoderRaw", robot.encoderTicksLeftDrive()); + SmartDashboard.putNumber("rightEncoderRaw", robot.encoderTicksRightDrive()); + 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); + if (tapeAlign) { switch (commandStep) { /////////////tapeAlign Code case -1: @@ -79,11 +98,11 @@ public void step(GenericRobot robot){ leftPower = defaultPower + correction; //didn't we stop doing this? rightPower = defaultPower - correction; - if (!robot.getTapeSensorOne()) { + if (!robot.getFloorSensorLeft()) { startDistance = robot.getDriveDistanceInchesLeft(); leftSensor = true; commandStep += 1; - } else if (!robot.getTapeSensorTwo()) { + } else if (!robot.getFloorSensorRight()) { startDistance = robot.getDriveDistanceInchesLeft(); rightSensor = true; commandStep += 1; @@ -94,37 +113,19 @@ public void step(GenericRobot robot){ leftPower = defaultPower + correction; //confusion rightPower = defaultPower - correction; - if (!rightSensor && !robot.getTapeSensorTwo()) { + if (!rightSensor && !robot.getFloorSensorRight()) { differenceDistance = Math.abs(robot.getDriveDistanceInchesLeft() - startDistance); Tapetheta = Math.atan(differenceDistance / sensorDist) * 180 / Math.PI; outerDistArc = robot.getDriveDistanceInchesRight(); commandStep += 1; - } else if (!leftSensor && !robot.getTapeSensorOne()) { + } else if (!leftSensor && !robot.getFloorSensorLeft()) { differenceDistance = Math.abs(robot.getDriveDistanceInchesLeft() - startDistance); Tapetheta = Math.atan(differenceDistance / sensorDist) * 180 / Math.PI; outerDistArc = robot.getDriveDistanceInchesLeft(); commandStep = 4; } break; - case 3://///////////////////////////////////////////////////////////////////skip this step - if (leftSensor) { - leftPower = defaultPower * .5; - rightPower = defaultPower * 2; - } else { - rightPower = defaultPower * .5; - leftPower = defaultPower * 2; - } - currentYaw = robot.getYaw(); - if (Math.abs(Math.signum(currentYaw - startAngle) * (((Math.abs(currentYaw - startAngle) + 180) % 360) - 180)) >= Math.abs(Tapetheta)) { - if (rightSensor) { - outerDistArc = robot.getDriveDistanceInchesLeft() - outerDistArc; - } else { - outerDistArc = robot.getDriveDistanceInchesRight() - outerDistArc; - } - lTraveled = Math.abs(outerDistArc / (Tapetheta * Math.PI / 180) * Math.sin(Math.abs(Tapetheta * Math.PI / 180))); - commandStep += 1; - }///////////////////////////////////////this step is skipped - break; + case 4: if (leftSensor) { currentYaw = startAngle - Tapetheta; //currentYaw = targetYaw because we are lazy @@ -149,61 +150,145 @@ public void step(GenericRobot robot){ case 6: //adios amigos leftPower = 0; rightPower = 0; - tapeAlign = false; + //tapeAlign = false; commandStep = 0; + startingTime = System.currentTimeMillis(); break; } robot.drivePercent(leftPower, rightPower); } else{////////////////////////////start the real stuff now +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// switch (commandStep){ case 0:///reset and enable PTO + //reset encoders robot.turnOnPTO(); - commandStep += 1; + robot.resetEncoders(); + countLeft = 0; + countRight = 0; + if (System.currentTimeMillis() - startingTime >= 50){ + commandStep += 1; + } + break; case 1: //////raise climber arms (skip 10 steps after in case we need to scoot/scoot - if (robot.armHeight() >= desiredHeight){ - robot.armPower(0); - commandStep += 1; + + if (!robot.getClimbSensorLeft() && countLeft == 0){ + countLeft = 1; + } + + if (robot.getClimbSensorLeft() && countLeft == 1){ + leftArmPower = 0; + leftArrived = true; + } + else{ + leftArmPower = defaultClimbPower; + } + + + if (!robot.getClimbSensorRight() && countRight == 0){ + countRight = 1; + } + + if (robot.getClimbSensorRight() && countRight == 1){ + rightArmPower = 0; + rightArrived = true; } else{ - robot.raiseClimberArms(); + rightArmPower = defaultClimbPower; + } + + if (leftArrived && rightArrived){ + leftArrived = false; + rightArrived = false; + countLeft = 0; + countRight = 0; + leftArmPower = 0; + rightArmPower = 0; + commandStep += 1; } + break; case 2: //////disable PTO robot.turnOffPTO(); commandStep += 1; break; - case 3: //////scoot back a lil - //go back, if in the right place, next step - if (robot.inTheRightPlace()){ + 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 (robot.getDriveDistanceInchesLeft() - startDistance <= -8){ + leftArmPower = 0; + rightArmPower = 0; commandStep += 1; - robot.armPower(0); // because this is really the drivetrain, it will work - } - else{ - robot.lowerClimberArms(); //because this is really the drivetrain, it will work } - break; - case 4: //////enable PTO + case 5: //////enable PTO robot.turnOnPTO(); commandStep = 11; break; case 11: ////////lower climber arms - if (robot.armInContact() && robot.armHeight() <= lowHeight){ - robot.armPower(0); - commandStep += 1; + + if (!robot.getClimbSensorLeft() && countLeft == 0){ + countLeft = 1; + } + + if (robot.getClimbSensorLeft() && countLeft == 1){ + leftArmPower = 0; + leftArrived = true; + } + else{ + leftArmPower = -defaultClimbPower; + } + + + if (!robot.getClimbSensorRight() && countRight == 0){ + countRight = 1; + } + + if (robot.getClimbSensorRight() && countRight == 1){ + rightArmPower = 0; + rightArrived = true; } else{ - robot.lowerClimberArms(); + rightArmPower = -defaultClimbPower; + } + if (robot.armInContact() && leftArrived && rightArrived){ + countRight = 0; + countLeft = 0; + leftArmPower = 0; + rightArmPower = 0; + commandStep += 1; + leftArrived = false; + rightArrived = false; + startHeightLeft = robot.armHeightLeft(); + startHeightRight = robot.armHeightRight(); + } break; case 12: /////////////raise arms slightly - if (robot.armHeight() >= escapeHeight && !robot.armInContact()){ - robot.armPower(0); - commandStep += 1; + if (Math.abs(robot.armHeightLeft()-startHeightLeft) >= escapeHeight){ + leftArmPower = 0; + leftArrived = true; + } + else{ + leftArmPower = .1; + } + if (Math.abs(robot.armHeightRight()-startHeightRight) >= escapeHeight){ + rightArmPower = 0; + rightArrived = true; } else{ - robot.raiseClimberArms(); + rightArmPower = .1; + } + if (rightArrived && leftArrived){ + rightArmPower = 0; + leftArmPower = 0; + rightArrived = false; + leftArrived = false; + commandStep += 1; } break; case 13: ///////////unlock rotation piston to send arms forward @@ -211,21 +296,53 @@ public void step(GenericRobot robot){ commandStep += 1; break; case 14: ///////////move arms forward - if (robot.armHeight() >= getToBarHeight){ - robot.armPower(0); - commandStep += 1; + + if (!robot.getClimbSensorLeft() && countLeft == 0){ + countLeft = 1; + } + + if (robot.getClimbSensorLeft() && countLeft == 1){ + leftArmPower = 0; + leftArrived = true; + } + else{ + leftArmPower = defaultClimbPower; + } + + + if (!robot.getClimbSensorRight() && countRight == 0){ + countRight = 1; + } + + if (robot.getClimbSensorRight() && countRight == 1){ + rightArmPower = 0; + rightArrived = true; } else{ - robot.raiseClimberArms(); + rightArmPower = defaultClimbPower; + } + + if (leftArrived && rightArrived){ + countLeft = 0; + countRight = 0; + rightArmPower = 0; + leftArmPower = 0; + rightArrived = false; + leftArrived = false; + commandStep = 16; //////////skip over step 15 } break; - case 15: - if (robot.armInContact()){ + 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; - robot.armPower(0); + leftArmPower = 0; + rightArmPower = 0; } else{ - robot.lowerClimberArms(); + leftArmPower = -.1; + rightArmPower = -.1; } case 16://///////once in contact move arms back again with the piston and swiiiiing robot.setArmsBackward(); @@ -234,26 +351,60 @@ public void step(GenericRobot robot){ case 17://////////go back to case 11 and repeat down to this step if (firstTime){ commandStep = 11; + countRight = 0; + countLeft = 0; firstTime = false; } else{ commandStep += 1; - robot.armPower(0); + leftArmPower = 0; + rightArmPower = 0; } break; - case 19:///////lift all the way up to be extra secure - if (robot.armHeight() <= lowHeight && robot.armInContact()){ - robot.armPower(0); - commandStep += 1; + 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 = -defaultClimbPower; + } + + + if (!robot.getClimbSensorRight() && countRight == 0){ + countRight = 1; + } + + if (robot.getClimbSensorRight() && countRight == 1){ + rightArmPower = 0; + rightArrived = true; } else{ - robot.lowerClimberArms(); + rightArmPower = -defaultClimbPower; + } + if (robot.armInContact() && leftArrived && rightArrived){ + countRight = 0; + countLeft = 0; + leftArmPower = 0; + rightArmPower = 0; + leftArrived = false; + rightArrived = false; + commandStep += 1; } - case 18: ////////now we are done. If all goes well, we are on the traversal rung, if not, we no longer have a robot >;( - robot.armPower(0); + + 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; + } + robot.armPower(leftArmPower, rightArmPower); } } } diff --git a/src/main/java/frc/robot/generic/GenericRobot.java b/src/main/java/frc/robot/generic/GenericRobot.java index c7924dd..8ec4550 100644 --- a/src/main/java/frc/robot/generic/GenericRobot.java +++ b/src/main/java/frc/robot/generic/GenericRobot.java @@ -48,6 +48,19 @@ public default double getDriveDistanceInchesRight(){ return encoderTicksRightDrive()/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(){ //System.out.println("I don't have an encoder"); return 1.0; @@ -108,6 +121,7 @@ public default void resetAttitude() { System.out.println("I don't have a navx"); } + public enum TeamColor { RED, BLUE, @@ -305,6 +319,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; @@ -357,10 +375,15 @@ public default void lowerClimberArms(){ public default void armPower(double power){ //System.out.println("Elbow Grease"); } - public default double armHeight(){ + + 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; @@ -372,4 +395,6 @@ public default boolean inTheRightPlace(){ //System.out.println("I don't have a climber"); } + + } diff --git a/src/main/java/frc/robot/generic/Lightning.java b/src/main/java/frc/robot/generic/Lightning.java index 18c9e9b..4139f38 100644 --- a/src/main/java/frc/robot/generic/Lightning.java +++ b/src/main/java/frc/robot/generic/Lightning.java @@ -74,6 +74,11 @@ public Lightning(){ rightMotorA.setInverted(invertRight); rightMotorB.setInverted(invertRight); + 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); @@ -126,6 +131,25 @@ public double getDriveRightRPM() { return encoderRight.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() { @@ -383,48 +407,40 @@ public void setArmsBackward(){ } @Override - public void raiseClimberArms() { - leftMotorB.set(.1); - leftMotorA.set(.1); - rightMotorA.set(.1); - rightMotorB.set(.1); - } - - @Override - public void lowerClimberArms() { - leftMotorB.set(-.1); - leftMotorA.set(-.1); - rightMotorA.set(-.1); - rightMotorB.set(-.1); + public void armPower(double leftPower, double rightPower) { + leftMotorB.set(leftPower); + leftMotorA.set(leftPower); + rightMotorA.set(rightPower); + rightMotorB.set(rightPower); } @Override - public void armPower(double power) { - leftMotorB.set(power); - leftMotorA.set(power); - rightMotorA.set(power); - rightMotorB.set(power); + public double armHeightLeft() { + //TODO: put in coversion + //Maybe use some sensor. Do NOT want to use encoders for this. + return encoderTicksLeftDrive(); } @Override - public double armHeight() { - //TODO - //Maybe use some sensor. Do NOT want to use encoders for this. - return GenericRobot.super.armHeight(); + public double armHeightRight(){ + return encoderTicksRightDrive(); } @Override public boolean armInContact() { //TODO: find tolerances - if (leftMotorA.getOutputCurrent() > LEFTATOLERANCE && leftMotorB.getOutputCurrent() > LEFTBTOLERANCE + /* 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?? @@ -483,15 +499,6 @@ public void resetAttitude() { navx.reset(); } - @Override - public boolean getTapeSensorOne() { - return false; - } - - @Override - public boolean getTapeSensorTwo() { - return false; - } @Override public double turretPIDgetP() { From e0b00caecc671e3567bd52f9f310ad6dff612a65 Mon Sep 17 00:00:00 2001 From: Louminator Date: Sun, 20 Feb 2022 23:48:32 -0500 Subject: [PATCH 15/72] Remove annoying .idea files from git repo. --- .gitignore | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 9de817b..24eba5d 100644 --- a/.gitignore +++ b/.gitignore @@ -238,4 +238,6 @@ fabric.properties .idea/httpRequests # Android studio 3.1+ serialized cache file -.idea/caches/build_file_checksums.ser \ No newline at end of file +.idea/caches/build_file_checksums.ser + +.idea/** From 3aff03766d0d938004cb8ce03d6b7c53b7d4033c Mon Sep 17 00:00:00 2001 From: DeathDIVer Date: Mon, 21 Feb 2022 19:25:20 -0500 Subject: [PATCH 16/72] Fixed degree --- .../frc/robot/autonomous/BallCtoTerminal.java | 73 ++++++++++++++++--- 1 file changed, 61 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/autonomous/BallCtoTerminal.java b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java index 986b993..2ff8b45 100644 --- a/src/main/java/frc/robot/autonomous/BallCtoTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java @@ -1,29 +1,41 @@ 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 +//Setup: 33.21 degrees from the white dividing line public class BallCtoTerminal extends GenericAutonomous { double startingYaw; - double startDistance; + double startTime; + double leftpower; double rightpower; double defaultPower = .4; double defaultTurnPower = .4; double correction; - double startTime; - double distanceC = 40.44; + double distanceC = 47.9; double distanceTerminal = 251; - double angleC = 84.54; + double angleC = 83.74; //og = 84.74 double rampDownDist = 10; PIDController PIDDriveStraight; - TurretTracker tracker = new TurretTracker(); + int averageTurretXSize = 2; + double[] averageTurretX = new double [averageTurretXSize]; + double turretx; + double turrety; + double turretarea; + double turretv; + int counter = 0; + PIDController turretPIDController; + //TurretTracker tracker = new TurretTracker(); @Override public void autonomousInit(GenericRobot robot) { @@ -31,12 +43,24 @@ public void autonomousInit(GenericRobot robot) { startingYaw = robot.getYaw(); startTime = System.currentTimeMillis(); PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD()); - tracker.turretInit(robot); + + turretPIDController = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD()); + //tracker.turretInit(robot); } @Override public void autonomousPeriodic(GenericRobot robot) { - tracker.turretUpdate(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); switch(autonomousStep){ case 0: //reset @@ -61,8 +85,10 @@ public void autonomousPeriodic(GenericRobot robot) { rightpower = defaultPower - correction; if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceC - rampDownDist){ - defaultPower = (distanceC-robot.getDriveDistanceInchesLeft()+startDistance) - *defaultPower/rampDownDist; + double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist, + robot.getDriveDistanceInchesLeft(), distanceC); + leftpower = ramp; + rightpower = ramp; } if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceC){ autonomousStep += 1; @@ -109,8 +135,10 @@ public void autonomousPeriodic(GenericRobot robot) { rightpower = defaultPower - correction; if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal - rampDownDist){ - defaultPower = (distanceTerminal-robot.getDriveDistanceInchesLeft()+startDistance) - *defaultPower/rampDownDist; + double ramp = rampDown(defaultPower, 0, startDistance, 10, + robot.getDriveDistanceInchesLeft(), distanceTerminal); + leftpower = ramp; + rightpower = ramp; } if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal){ autonomousStep += 1; @@ -123,6 +151,27 @@ public void autonomousPeriodic(GenericRobot robot) { break; } robot.drivePercent(leftpower, rightpower); - tracker.turretMove(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); + //tracker.turretMove(robot); } } From d9619e1f70e831ecefb7e9747a22ac8ac64878f7 Mon Sep 17 00:00:00 2001 From: CoolSakamoto Date: Mon, 21 Feb 2022 19:35:40 -0500 Subject: [PATCH 17/72] Coded movement and testing of linear actuators on Lightning hood (to control pitch), code done by Jack on programming Laptop --- .idea/misc.xml | 2 +- src/main/java/frc/robot/Robot.java | 12 ++++++--- .../java/frc/robot/generic/GenericRobot.java | 8 ++++++ .../java/frc/robot/generic/Lightning.java | 27 ++++++++++++++++--- 4 files changed, 42 insertions(+), 7 deletions(-) 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/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f016cfc..fcd0ad8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -303,15 +303,21 @@ else if (joystick.getRawButton(5)){ @Override public void testPeriodic() { + double pitchChange = 0; if (xbox.getRawButton(1)){ - robot.setTurretPitchPowerPct(1); + pitchChange = 0.02; } else if (xbox.getRawButton(2)){ - robot.setTurretPitchPowerPct(-1); + pitchChange = -0.02; } else{ - robot.setTurretPitchPowerPct(0); + pitchChange = 0; } + double newPos = robot.getTurretPitchPosition() + pitchChange; + if(newPos < 0) newPos = 0; + if(newPos > 1) newPos = 1; + + robot.setTurretPitchPosition(newPos); double driveX = joystick.getX(); double driveY = -joystick.getY(); diff --git a/src/main/java/frc/robot/generic/GenericRobot.java b/src/main/java/frc/robot/generic/GenericRobot.java index 4139de6..33ae328 100644 --- a/src/main/java/frc/robot/generic/GenericRobot.java +++ b/src/main/java/frc/robot/generic/GenericRobot.java @@ -219,6 +219,10 @@ public default double getTurretPowerPct(){ return 0; } + public default double getTurretPitchPosition(){ + return 0; + } + public default double getTurretPitchAngle(){ return 0; } @@ -226,6 +230,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"); } diff --git a/src/main/java/frc/robot/generic/Lightning.java b/src/main/java/frc/robot/generic/Lightning.java index fe79b42..4f3f018 100644 --- a/src/main/java/frc/robot/generic/Lightning.java +++ b/src/main/java/frc/robot/generic/Lightning.java @@ -26,7 +26,11 @@ public class Lightning implements GenericRobot { CANSparkMax rightMotorA = new CANSparkMax(18, kBrushless); CANSparkMax rightMotorB = new CANSparkMax(19, kBrushless); - Servo elevationLeft = new Servo(9); + //TODO: update servo ports + //servo left was initially set to channel 9, don't know if that means anything + Servo elevationLeft = new Servo(0); + Servo elevationRight = new Servo(1); + RelativeEncoder encoderRight = rightMotorA.getEncoder(); RelativeEncoder encoderLeft = leftMotorA.getEncoder(); @@ -35,6 +39,7 @@ public class Lightning implements GenericRobot { 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); @@ -245,8 +250,24 @@ public double getTurretPowerPct() { } @Override - public void setTurretPitchPowerPct(double speed){ - elevationLeft.setSpeed(speed); + public double getTurretPitchPosition(){ + elevationLeft.setBounds(2.0, 1.8, 1.5, 1.2, 1.0); + elevationRight.setBounds(2.0, 1.8, 1.5, 1.2, 1.0); + + //TODO: getSpeed()? getPosition()? getAngle()? don't know which to use + return elevationLeft.getSpeed(); + } + @Override + public void setTurretPitchPosition(double position){ + if(position < 0) position = 0; + if(position > 1) position = 1; + + elevationLeft.setBounds(2.0, 1.8, 1.5, 1.2, 1.0); + elevationRight.setBounds(2.0, 1.8, 1.5, 1.2, 1.0); + + //TODO: figure out use setSpeed() or set() + elevationLeft.setSpeed(position); + elevationRight.setSpeed(position); } From 9350acc9caf5a405a28702c83270b54214cadcc9 Mon Sep 17 00:00:00 2001 From: DeathDIVer Date: Mon, 21 Feb 2022 20:05:34 -0500 Subject: [PATCH 18/72] Finished BallCtoTerminal --- .../frc/robot/autonomous/BallCtoTerminal.java | 37 ++++++++++++++----- .../robot/autonomous/GenericAutonomous.java | 2 +- 2 files changed, 29 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/autonomous/BallCtoTerminal.java b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java index 2ff8b45..cbf743c 100644 --- a/src/main/java/frc/robot/autonomous/BallCtoTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java @@ -22,7 +22,7 @@ public class BallCtoTerminal extends GenericAutonomous { double distanceC = 47.9; double distanceTerminal = 251; - double angleC = 83.74; //og = 84.74 + double angleC = 82.74; //og = 84.74 double rampDownDist = 10; PIDController PIDDriveStraight; @@ -34,7 +34,9 @@ public class BallCtoTerminal extends GenericAutonomous { double turretarea; double turretv; int counter = 0; + boolean time = false; PIDController turretPIDController; + PIDController PIDPivot; //TurretTracker tracker = new TurretTracker(); @Override @@ -42,9 +44,11 @@ public void autonomousInit(GenericRobot robot) { autonomousStep = 0; startingYaw = robot.getYaw(); startTime = System.currentTimeMillis(); - PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD()); + PIDDriveStraight = 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()); + //tracker.turretInit(robot); } @@ -65,6 +69,8 @@ public void autonomousPeriodic(GenericRobot robot) { switch(autonomousStep){ case 0: //reset robot.lowerCollector(); + PIDPivot.reset(); + PIDPivot.enableContinuousInput(-180,180); PIDDriveStraight.reset(); PIDDriveStraight.enableContinuousInput(-180,180); robot.resetEncoders(); @@ -100,6 +106,7 @@ public void autonomousPeriodic(GenericRobot robot) { rightpower = 0; startDistance = robot.getDriveDistanceInchesLeft(); autonomousStep = 12; + time = false; break; case 6: //collector to collect ball case 7: //collection part 2 not electric nor boogaloo @@ -117,15 +124,27 @@ public void autonomousPeriodic(GenericRobot robot) { } break; case 13: //turn to go to ball @ terminal - leftpower = -defaultTurnPower; - rightpower = defaultTurnPower; + correction = PIDPivot.calculate(angleC + robot.getYaw() - startingYaw ); + leftpower = correction; + rightpower = -correction; //turning left - - if(robot.getYaw() - startingYaw < -angleC) { - startingYaw = startingYaw - angleC; - startDistance = robot.getDriveDistanceInchesLeft(); - PIDDriveStraight.reset(); + 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 14: //drive towards the ball diff --git a/src/main/java/frc/robot/autonomous/GenericAutonomous.java b/src/main/java/frc/robot/autonomous/GenericAutonomous.java index 81a61c2..15c93ce 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 :'("); From c0abf9e76822d5f9a265aaa7b1ad03a9224d31b3 Mon Sep 17 00:00:00 2001 From: Malti Date: Mon, 21 Feb 2022 21:02:09 -0500 Subject: [PATCH 19/72] Added am arm up and down function. Hopefully the robot will not be broken so we can test. --- src/main/java/frc/robot/command/Hang.java | 31 ++++++++++--------- .../java/frc/robot/generic/GenericRobot.java | 4 +-- .../java/frc/robot/generic/Lightning.java | 19 +++++++++--- 3 files changed, 34 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/command/Hang.java b/src/main/java/frc/robot/command/Hang.java index 5930330..a15bdb6 100644 --- a/src/main/java/frc/robot/command/Hang.java +++ b/src/main/java/frc/robot/command/Hang.java @@ -48,7 +48,8 @@ public class Hang extends GenericCommand{ int countRight = 0; double leftArmPower = 0; double rightArmPower = 0; - double defaultClimbPower = .1; + double defaultClimbPowerUp = .5; + double defaultClimbPowerDown = -.2; boolean leftArrived = false; boolean rightArrived = false; double startHeightLeft = 0; @@ -150,7 +151,7 @@ public void step(GenericRobot robot){ case 6: //adios amigos leftPower = 0; rightPower = 0; - //tapeAlign = false; + //tapeAlign = false; TODO: make this stop being a comment commandStep = 0; startingTime = System.currentTimeMillis(); break; @@ -182,7 +183,7 @@ public void step(GenericRobot robot){ leftArrived = true; } else{ - leftArmPower = defaultClimbPower; + leftArmPower = defaultClimbPowerUp; } @@ -195,7 +196,7 @@ public void step(GenericRobot robot){ rightArrived = true; } else{ - rightArmPower = defaultClimbPower; + rightArmPower = defaultClimbPowerUp; } if (leftArrived && rightArrived){ @@ -205,7 +206,8 @@ public void step(GenericRobot robot){ countRight = 0; leftArmPower = 0; rightArmPower = 0; - commandStep += 1; + commandStep = 11; + ////////////just for testing TODO: put it back to normal(+=1) } break; @@ -240,7 +242,7 @@ public void step(GenericRobot robot){ leftArrived = true; } else{ - leftArmPower = -defaultClimbPower; + leftArmPower = defaultClimbPowerDown; } @@ -253,14 +255,14 @@ public void step(GenericRobot robot){ rightArrived = true; } else{ - rightArmPower = -defaultClimbPower; + rightArmPower = defaultClimbPowerDown; } if (robot.armInContact() && leftArrived && rightArrived){ countRight = 0; countLeft = 0; leftArmPower = 0; rightArmPower = 0; - commandStep += 1; + commandStep = 19;//TODO: put it back to normal(+=1) leftArrived = false; rightArrived = false; startHeightLeft = robot.armHeightLeft(); @@ -274,14 +276,14 @@ public void step(GenericRobot robot){ leftArrived = true; } else{ - leftArmPower = .1; + leftArmPower = defaultClimbPowerUp; } if (Math.abs(robot.armHeightRight()-startHeightRight) >= escapeHeight){ rightArmPower = 0; rightArrived = true; } else{ - rightArmPower = .1; + rightArmPower = defaultClimbPowerUp; } if (rightArrived && leftArrived){ rightArmPower = 0; @@ -306,7 +308,7 @@ public void step(GenericRobot robot){ leftArrived = true; } else{ - leftArmPower = defaultClimbPower; + leftArmPower = defaultClimbPowerUp; } @@ -319,7 +321,7 @@ public void step(GenericRobot robot){ rightArrived = true; } else{ - rightArmPower = defaultClimbPower; + rightArmPower = defaultClimbPowerUp; } if (leftArrived && rightArrived){ @@ -372,7 +374,7 @@ public void step(GenericRobot robot){ leftArrived = true; } else{ - leftArmPower = -defaultClimbPower; + leftArmPower = defaultClimbPowerDown; } @@ -385,8 +387,9 @@ public void step(GenericRobot robot){ rightArrived = true; } else{ - rightArmPower = -defaultClimbPower; + rightArmPower = defaultClimbPowerDown; } + if (robot.armInContact() && leftArrived && rightArrived){ countRight = 0; countLeft = 0; diff --git a/src/main/java/frc/robot/generic/GenericRobot.java b/src/main/java/frc/robot/generic/GenericRobot.java index 8ec4550..66647cd 100644 --- a/src/main/java/frc/robot/generic/GenericRobot.java +++ b/src/main/java/frc/robot/generic/GenericRobot.java @@ -366,10 +366,10 @@ public default boolean isActivelyShooting(){ public default void setActivelyShooting(boolean isShooting){ } - public default void raiseClimberArms(){ + public default void raiseClimberArms(double rightPower, double leftPower){ //System.out.println("I don't have a climber"); } - public default void lowerClimberArms(){ + public default void lowerClimberArms(double rightPower, double leftPower){ //System.out.println("I don't have a climber"); } public default void armPower(double power){ diff --git a/src/main/java/frc/robot/generic/Lightning.java b/src/main/java/frc/robot/generic/Lightning.java index 4139f38..e4b31e6 100644 --- a/src/main/java/frc/robot/generic/Lightning.java +++ b/src/main/java/frc/robot/generic/Lightning.java @@ -413,16 +413,27 @@ public void armPower(double leftPower, double rightPower) { 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 coversion + //TODO: put in conversion //Maybe use some sensor. Do NOT want to use encoders for this. return encoderTicksLeftDrive(); } @Override public double armHeightRight(){ + //TODO: put in conversion return encoderTicksRightDrive(); } @@ -458,17 +469,17 @@ 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 From debfd555400357a49f0ab6d945567ae780f6ab83 Mon Sep 17 00:00:00 2001 From: DeathDIVer Date: Tue, 22 Feb 2022 09:38:03 -0500 Subject: [PATCH 20/72] made tweaks to some of the code, BallSimpleC, autoArc, BallBtoTerminal, and BallCtoTerminal should now all be finished and ready to go. --- src/main/java/frc/robot/Robot.java | 2 +- .../java/frc/robot/autonomous/BallAtoB.java | 2 +- .../robot/autonomous/BallAtoBtoTerminal.java | 2 +- .../frc/robot/autonomous/BallBtoTerminal.java | 12 ++++-- .../frc/robot/autonomous/BallCtoTerminal.java | 20 ++++----- .../frc/robot/autonomous/BallSimpleB.java | 11 +++-- .../{BallSimple.java => BallSimpleC.java} | 42 +++++++++++++++---- 7 files changed, 60 insertions(+), 31 deletions(-) rename src/main/java/frc/robot/autonomous/{BallSimple.java => BallSimpleC.java} (62%) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 92fa969..cd1ef8c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -30,7 +30,7 @@ public class Robot extends TimedRobot { GenericRobot robot = new TurretBot(); Joystick joystick = new Joystick(0); Joystick xbox = new Joystick(1); - GenericAutonomous autonomous = simpleCTerminal; + GenericAutonomous autonomous = autoArc; int averageTurretXSize = 2; diff --git a/src/main/java/frc/robot/autonomous/BallAtoB.java b/src/main/java/frc/robot/autonomous/BallAtoB.java index 7b28128..ebfc432 100644 --- a/src/main/java/frc/robot/autonomous/BallAtoB.java +++ b/src/main/java/frc/robot/autonomous/BallAtoB.java @@ -20,7 +20,7 @@ public class BallAtoB extends GenericAutonomous { double distanceA = 40.44; double distanceB = 259.26; //CHANGE double angleA = 87.74; - + double angleB = 0; //change double rampDownDist = 10; diff --git a/src/main/java/frc/robot/autonomous/BallAtoBtoTerminal.java b/src/main/java/frc/robot/autonomous/BallAtoBtoTerminal.java index 3ae195f..c6c2041 100644 --- a/src/main/java/frc/robot/autonomous/BallAtoBtoTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallAtoBtoTerminal.java @@ -82,7 +82,7 @@ public void autonomousPeriodic(GenericRobot robot) { case 12: //turn to go to ball B leftpower = defaultTurnPower; rightpower = -defaultTurnPower; - //turning left + //turning right if(robot.getYaw() - startingYaw > angleA) { startingYaw = startingYaw + angleA; diff --git a/src/main/java/frc/robot/autonomous/BallBtoTerminal.java b/src/main/java/frc/robot/autonomous/BallBtoTerminal.java index 7766418..ec5e21e 100644 --- a/src/main/java/frc/robot/autonomous/BallBtoTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallBtoTerminal.java @@ -4,7 +4,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.generic.GenericRobot; -//Simple autonomous code for ball A, closest ball to the hangar +//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; @@ -16,7 +17,7 @@ public class BallBtoTerminal extends GenericAutonomous { double correction; double distanceB = 61.5; - double distanceTerminal = 0; + double distanceTerminal = 160.6; double rampDownDist = 10; @@ -49,14 +50,17 @@ public void autonomousPeriodic(GenericRobot robot) { 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 + case 4: //drive to 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; + double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist, + robot.getDriveDistanceInchesLeft(), distanceB); + leftpower = ramp; + rightpower = ramp; } if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceB){ autonomousStep += 1; diff --git a/src/main/java/frc/robot/autonomous/BallCtoTerminal.java b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java index cbf743c..7ec04dd 100644 --- a/src/main/java/frc/robot/autonomous/BallCtoTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java @@ -4,11 +4,10 @@ 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 -//Setup: 33.21 degrees from the white dividing line +//Setup: Line the robot straight between ball C and the center point of the hub public class BallCtoTerminal extends GenericAutonomous { double startingYaw; double startDistance; @@ -19,13 +18,16 @@ public class BallCtoTerminal extends GenericAutonomous { double defaultPower = .4; double defaultTurnPower = .4; double correction; + boolean time = false; double distanceC = 47.9; double distanceTerminal = 251; - double angleC = 82.74; //og = 84.74 + double angleC = 82.74; double rampDownDist = 10; PIDController PIDDriveStraight; + PIDController PIDTurret; + PIDController PIDPivot; int averageTurretXSize = 2; double[] averageTurretX = new double [averageTurretXSize]; @@ -34,9 +36,7 @@ public class BallCtoTerminal extends GenericAutonomous { double turretarea; double turretv; int counter = 0; - boolean time = false; - PIDController turretPIDController; - PIDController PIDPivot; + //TurretTracker tracker = new TurretTracker(); @Override @@ -47,7 +47,7 @@ public void autonomousInit(GenericRobot robot) { PIDDriveStraight = 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()); + PIDTurret = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD()); //tracker.turretInit(robot); } @@ -116,12 +116,10 @@ public void autonomousPeriodic(GenericRobot robot) { 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: //turn to go to ball @ terminal correction = PIDPivot.calculate(angleC + robot.getYaw() - startingYaw ); @@ -185,9 +183,9 @@ public void autonomousPeriodic(GenericRobot robot) { double currentTurretPower = 0; if(turretv !=0){ - currentTurretPower = turretPIDController.calculate(average); + currentTurretPower = PIDTurret.calculate(average); }else{ - turretPIDController.reset(); + PIDTurret.reset(); } robot.setTurretPowerPct(currentTurretPower); diff --git a/src/main/java/frc/robot/autonomous/BallSimpleB.java b/src/main/java/frc/robot/autonomous/BallSimpleB.java index 04f16b3..aba6cee 100644 --- a/src/main/java/frc/robot/autonomous/BallSimpleB.java +++ b/src/main/java/frc/robot/autonomous/BallSimpleB.java @@ -1,10 +1,9 @@ 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 +//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; @@ -15,7 +14,7 @@ public class BallSimpleB extends GenericAutonomous { double correction; double startTime; - double distance = 61.5; //distance to ball B from the tarmac + double distanceB = 61.5; //distance to ball B from the tarmac double rampDownDist = 10; PIDController PIDDriveStraight; @@ -53,11 +52,11 @@ public void autonomousPeriodic(GenericRobot robot){ leftpower = defaultPower + correction; rightpower = defaultPower - correction; - if(robot.getDriveDistanceInchesLeft() - startDistance >= distance - rampDownDist){ - defaultPower = (distance - robot.getDriveDistanceInchesLeft() + startDistance) + if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceB - rampDownDist){ + defaultPower = (distanceB - robot.getDriveDistanceInchesLeft() + startDistance) * defaultPower / rampDownDist; } - if(robot.getDriveDistanceInchesLeft() - startDistance >= distance){ + if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceB){ autonomousStep += 1; startTime = System.currentTimeMillis(); } diff --git a/src/main/java/frc/robot/autonomous/BallSimple.java b/src/main/java/frc/robot/autonomous/BallSimpleC.java similarity index 62% rename from src/main/java/frc/robot/autonomous/BallSimple.java rename to src/main/java/frc/robot/autonomous/BallSimpleC.java index 3ed5203..fc2ff8a 100644 --- a/src/main/java/frc/robot/autonomous/BallSimple.java +++ b/src/main/java/frc/robot/autonomous/BallSimpleC.java @@ -4,8 +4,9 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.generic.GenericRobot; -//Simple autonomous code for ball A and C -public class BallSimple extends GenericAutonomous { +//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; @@ -15,7 +16,8 @@ public class BallSimple extends GenericAutonomous { double correction; double startTime; - double distance = 40.44; //either to Balls A, B or C depending on where the robot is positioned + double distanceC = 47.9; + double distance = 10; double rampDownDist = 10; PIDController PIDDriveStraight; @@ -53,11 +55,13 @@ public void autonomousPeriodic(GenericRobot robot){ leftpower = defaultPower + correction; rightpower = defaultPower - correction; - if(robot.getDriveDistanceInchesLeft() - startDistance >= distance - rampDownDist){ - defaultPower = (distance - robot.getDriveDistanceInchesLeft() + startDistance) - * defaultPower / rampDownDist; + if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceC - rampDownDist){ + double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist, + robot.getDriveDistanceInchesLeft(), distanceC); + leftpower = ramp; + rightpower = ramp; } - if(robot.getDriveDistanceInchesLeft() - startDistance >= distance){ + if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceC){ autonomousStep += 1; startTime = System.currentTimeMillis(); } @@ -69,6 +73,30 @@ public void autonomousPeriodic(GenericRobot robot){ 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); } From 30266a240afe48334a08ebe943c8e6c6186b749f Mon Sep 17 00:00:00 2001 From: Malti Date: Tue, 22 Feb 2022 18:36:15 -0500 Subject: [PATCH 21/72] Merged collectorLogic with hangar_climb --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4be7e0e..97ba503 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -28,7 +28,7 @@ public class Robot extends TimedRobot { simpleBTerminal = new SimpleBTerminal(), simpleCTerminal = new SimpleCTerminal(); - GenericRobot robot = new TurretBot(); + GenericRobot robot = new Lightning(); Joystick joystick = new Joystick(0); GenericCommand command = new Hang(); Joystick xbox = new Joystick(1); From 73f397c762828a0f19eab20a99c9ec15c857a54f Mon Sep 17 00:00:00 2001 From: Malti Date: Tue, 22 Feb 2022 19:30:51 -0500 Subject: [PATCH 22/72] Tests from climb --- src/main/java/frc/robot/Robot.java | 14 +++++++++++++- .../java/frc/robot/command/GenericCommand.java | 1 + src/main/java/frc/robot/command/Hang.java | 12 +++++++----- src/main/java/frc/robot/generic/Lightning.java | 4 ++-- 4 files changed, 23 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 97ba503..6defa33 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -52,6 +52,7 @@ public class Robot extends TimedRobot { double maxCurrentLeftB = 0; double maxCurrentRightA = 0; double maxCurrentRightB = 0; + boolean ActuallyHanging = false; PIDController turretPIDController; @@ -175,6 +176,13 @@ public class Robot extends TimedRobot { 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("CommandStep", command.commandStep); + } @@ -265,7 +273,9 @@ else if(xbox.getRawAxis(rightAxis) < -tolerance){ } } - else{ + if(hang){ + ActuallyHanging = true; + SmartDashboard.putBoolean("we really are hanging", ActuallyHanging); if (reset){ command.begin(robot); reset = false; @@ -347,6 +357,8 @@ else if (joystick.getRawButton(5)){ @Override public void disabledInit() {} @Override public void disabledPeriodic() { + hang = false; + count = 0; if (joystick.getRawButton(1)){ robot.resetAttitude(); robot.resetEncoders(); diff --git a/src/main/java/frc/robot/command/GenericCommand.java b/src/main/java/frc/robot/command/GenericCommand.java index b7ca72d..ed28f0e 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 :'("); diff --git a/src/main/java/frc/robot/command/Hang.java b/src/main/java/frc/robot/command/Hang.java index a15bdb6..6f3a3f0 100644 --- a/src/main/java/frc/robot/command/Hang.java +++ b/src/main/java/frc/robot/command/Hang.java @@ -32,7 +32,6 @@ public class Hang extends GenericCommand{ double fwd = 48; PIDController PIDSteering; - int commandStep = -1; boolean tapeAlign; /////^^^^^^^^^^^Stuff for tapeAlign @@ -48,8 +47,8 @@ public class Hang extends GenericCommand{ int countRight = 0; double leftArmPower = 0; double rightArmPower = 0; - double defaultClimbPowerUp = .5; - double defaultClimbPowerDown = -.2; + double defaultClimbPowerUp = -.5; + double defaultClimbPowerDown = .2; boolean leftArrived = false; boolean rightArrived = false; double startHeightLeft = 0; @@ -64,7 +63,7 @@ public void begin(GenericRobot robot){ lTraveled = 0; fwd = 75.6; PIDSteering = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD()); - tapeAlign = true; + tapeAlign = false; firstTime = true; } @@ -161,6 +160,9 @@ public void step(GenericRobot robot){ else{////////////////////////////start the real stuff now /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// switch (commandStep){ + case -1: + commandStep += 1; + break; case 0:///reset and enable PTO //reset encoders robot.turnOnPTO(); @@ -168,7 +170,7 @@ public void step(GenericRobot robot){ countLeft = 0; countRight = 0; if (System.currentTimeMillis() - startingTime >= 50){ - commandStep += 1; + commandStep = 11;//TODO: fix } break; diff --git a/src/main/java/frc/robot/generic/Lightning.java b/src/main/java/frc/robot/generic/Lightning.java index 7917fec..1584948 100644 --- a/src/main/java/frc/robot/generic/Lightning.java +++ b/src/main/java/frc/robot/generic/Lightning.java @@ -428,13 +428,13 @@ public void lowerClimberArms(double rightPower, double leftPower){ public double armHeightLeft() { //TODO: put in conversion //Maybe use some sensor. Do NOT want to use encoders for this. - return encoderTicksLeftDrive(); + return -encoderTicksLeftDrive()*.24937; } @Override public double armHeightRight(){ //TODO: put in conversion - return encoderTicksRightDrive(); + return -encoderTicksRightDrive()*.24937; } @Override From 3c0798be13e8958b1ea74212629552101e0039b9 Mon Sep 17 00:00:00 2001 From: Malti Date: Tue, 22 Feb 2022 21:04:09 -0500 Subject: [PATCH 23/72] Climb tests. TapeAlign is being weird :( --- src/main/java/frc/robot/Robot.java | 3 +- src/main/java/frc/robot/command/Hang.java | 60 +++++++++++++++++------ 2 files changed, 46 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6defa33..4abb9e1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -180,6 +180,7 @@ public class Robot extends TimedRobot { SmartDashboard.putNumber("leftArmEncoder", robot.armHeightLeft()); SmartDashboard.putBoolean("hang", hang); + SmartDashboard.putNumber("count", count); SmartDashboard.putNumber("CommandStep", command.commandStep); @@ -203,7 +204,7 @@ public class Robot extends TimedRobot { @Override public void teleopPeriodic() { //note to self: buttons currently assume mirrored joystick setting - if (joystick.getRawButtonPressed(6)){ + if (joystick.getRawButtonPressed(8)){ count = (count+1)%2; } if (count == 1){ diff --git a/src/main/java/frc/robot/command/Hang.java b/src/main/java/frc/robot/command/Hang.java index 6f3a3f0..eda43ad 100644 --- a/src/main/java/frc/robot/command/Hang.java +++ b/src/main/java/frc/robot/command/Hang.java @@ -48,7 +48,7 @@ public class Hang extends GenericCommand{ double leftArmPower = 0; double rightArmPower = 0; double defaultClimbPowerUp = -.5; - double defaultClimbPowerDown = .2; + double defaultClimbPowerDown = .5; boolean leftArrived = false; boolean rightArrived = false; double startHeightLeft = 0; @@ -61,13 +61,16 @@ public void begin(GenericRobot robot){ leftSensor = false; rightSensor = false; lTraveled = 0; - fwd = 75.6; + fwd = 71.6; PIDSteering = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD()); - tapeAlign = false; + 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.encoderTicksLeftDrive()); SmartDashboard.putNumber("rightEncoderRaw", robot.encoderTicksRightDrive()); SmartDashboard.putBoolean("leftTapeSensor", robot.getFloorSensorLeft()); @@ -78,6 +81,7 @@ public void step(GenericRobot robot){ SmartDashboard.putNumber("countRight", countRight); if (tapeAlign) { + switch (commandStep) { /////////////tapeAlign Code case -1: robot.resetEncoders(); @@ -117,7 +121,7 @@ public void step(GenericRobot robot){ differenceDistance = Math.abs(robot.getDriveDistanceInchesLeft() - startDistance); Tapetheta = Math.atan(differenceDistance / sensorDist) * 180 / Math.PI; outerDistArc = robot.getDriveDistanceInchesRight(); - commandStep += 1; + commandStep = 4; } else if (!leftSensor && !robot.getFloorSensorLeft()) { differenceDistance = Math.abs(robot.getDriveDistanceInchesLeft() - startDistance); Tapetheta = Math.atan(differenceDistance / sensorDist) * 180 / Math.PI; @@ -151,7 +155,7 @@ public void step(GenericRobot robot){ leftPower = 0; rightPower = 0; //tapeAlign = false; TODO: make this stop being a comment - commandStep = 0; + //commandStep = -1; startingTime = System.currentTimeMillis(); break; } @@ -170,11 +174,15 @@ public void step(GenericRobot robot){ countLeft = 0; countRight = 0; if (System.currentTimeMillis() - startingTime >= 50){ - commandStep = 11;//TODO: fix + commandStep += 1; } break; - case 1: //////raise climber arms (skip 10 steps after in case we need to scoot/scoot + case 1: ///////////unlock rotation piston to send arms forward + robot.setArmsForward(); + 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; @@ -208,12 +216,16 @@ public void step(GenericRobot robot){ countRight = 0; leftArmPower = 0; rightArmPower = 0; - commandStep = 11; - ////////////just for testing TODO: put it back to normal(+=1) + commandStep += 1; + } break; - case 2: //////disable PTO + case 3: ///////////unlock rotation piston to send arms forward + robot.setArmsBackward(); + commandStep = 11; + break; + /*case 2: //////disable PTO robot.turnOffPTO(); commandStep += 1; break; @@ -224,7 +236,7 @@ public void step(GenericRobot robot){ case 4: //go back 8 in leftArmPower = -.1; //see if we should change to drive stuff rightArmPower = -.1; - if (robot.getDriveDistanceInchesLeft() - startDistance <= -8){ + if (Math.abs(robot.getDriveDistanceInchesLeft() - startDistance) >= 8){ leftArmPower = 0; rightArmPower = 0; commandStep += 1; @@ -232,7 +244,7 @@ public void step(GenericRobot robot){ case 5: //////enable PTO robot.turnOnPTO(); commandStep = 11; - break; + break;*/ case 11: ////////lower climber arms if (!robot.getClimbSensorLeft() && countLeft == 0){ @@ -264,14 +276,26 @@ public void step(GenericRobot robot){ countLeft = 0; leftArmPower = 0; rightArmPower = 0; - commandStep = 19;//TODO: put it back to normal(+=1) + //TODO: change + if (!firstTime){ + commandStep = 19; + } + else { + commandStep = 30; + } leftArrived = false; rightArrived = false; startHeightLeft = robot.armHeightLeft(); startHeightRight = robot.armHeightRight(); + startingTime = System.currentTimeMillis(); } break; + case 30: //delay :)TODO:change + if (System.currentTimeMillis() - startingTime >= 5000){ + commandStep = 12; + } + break; case 12: /////////////raise arms slightly if (Math.abs(robot.armHeightLeft()-startHeightLeft) >= escapeHeight){ leftArmPower = 0; @@ -331,8 +355,6 @@ public void step(GenericRobot robot){ countRight = 0; rightArmPower = 0; leftArmPower = 0; - rightArrived = false; - leftArrived = false; commandStep = 16; //////////skip over step 15 } break; @@ -343,6 +365,8 @@ public void step(GenericRobot robot){ commandStep += 1; leftArmPower = 0; rightArmPower = 0; + rightArrived = false; + leftArrived = false; } else{ leftArmPower = -.1; @@ -350,18 +374,22 @@ public void step(GenericRobot robot){ } case 16://///////once in contact move arms back again with the piston and swiiiiing robot.setArmsBackward(); - commandStep += 1; + 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; From 3a168c45d3cd6594840669fdc607ffa479bf2f01 Mon Sep 17 00:00:00 2001 From: JW314 Date: Thu, 24 Feb 2022 16:34:02 -0500 Subject: [PATCH 24/72] Main and Linear Actuator merge, plus some misc bug fixes --- .idea/misc.xml | 2 +- src/main/java/frc/robot/Robot.java | 64 +++++++++++-------- .../java/frc/robot/generic/Lightning.java | 8 ++- 3 files changed, 44 insertions(+), 30 deletions(-) diff --git a/.idea/misc.xml b/.idea/misc.xml index b808dca..1c80b4e 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -4,5 +4,5 @@ - + \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index df0a3c6..92ce2e2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -27,7 +27,7 @@ public class Robot extends TimedRobot { simpleBTerminal = new SimpleBTerminal(), simpleCTerminal = new SimpleCTerminal(); - GenericRobot robot = new TurretBot(); + GenericRobot robot = new Lightning(); Joystick joystick = new Joystick(0); Joystick xbox = new Joystick(1); GenericAutonomous autonomous = autoArc; @@ -36,10 +36,10 @@ public class Robot extends TimedRobot { int averageTurretXSize = 2; double[] averageX = new double [averageTurretXSize]; - double turretx; + /*double turretx; double turrety; double turretarea; - double turretv; + double turretv;*/ int counter = 0; double average; double currentTurretPower; @@ -53,7 +53,7 @@ public class Robot extends TimedRobot { @Override public void robotPeriodic() { - NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); + /*NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); NetworkTableEntry tx = table.getEntry("tx"); NetworkTableEntry ty = table.getEntry("ty"); NetworkTableEntry ta = table.getEntry("ta"); @@ -63,13 +63,13 @@ public class Robot extends TimedRobot { turretx = tx.getDouble(0.0); turrety = ty.getDouble(0.0); turretarea = ta.getDouble(0.0); - turretv = tv.getDouble(0.0); + turretv = tv.getDouble(0.0);*/ - SmartDashboard.putNumber("tv", turretv); + SmartDashboard.putBoolean("tv", robot.isTargetFound()); - SmartDashboard.putNumber("LimelightX", turretx); - SmartDashboard.putNumber("LimelightY", turrety); - SmartDashboard.putNumber("LimelightArea", turretarea); + 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()); @@ -116,7 +116,7 @@ public class Robot extends TimedRobot { 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()); @@ -177,25 +177,13 @@ public class Robot extends TimedRobot { } - 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)); - - - //currently Jack has no clue what axises these are supposed to be + double driveLeft = 0; + double driveRight = 0; 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; @@ -232,7 +220,7 @@ else if(xbox.getRawAxis(rightAxis) < -tolerance){ SmartDashboard.putNumber("Average", average); if (joystick.getRawButtonPressed(1)) turretPIDController.reset(); - if (joystick.getRawButton(1) && turretv !=0){ + if (joystick.getRawButton(1) && robot.isTargetFound()){ currentTurretPower = turretPIDController.calculate(average); } else { if (joystick.getRawButton(3)) currentTurretPower = -0.1; @@ -240,10 +228,13 @@ else if(xbox.getRawAxis(rightAxis) < -tolerance){ else currentTurretPower = 0.0; } + if (joystick.getRawButton(12)) currentTurretPower = 0.2; + else if (joystick.getRawButton(15)) currentTurretPower = -0.2; + robot.setTurretPowerPct(currentTurretPower); double shooterTargetRPM = 0; - if (joystick.getRawButton(10)){ + if (joystick.getRawButton(11)){ shooterTargetRPM = robot.getShooterTargetRPM(); }else{ shooterTargetRPM = 0; @@ -263,6 +254,27 @@ else if(xbox.getRawAxis(rightAxis) < -tolerance){ robot.setActivelyShooting(false); } + //force override rpm check + if(joystick.getRawButton(7)){ + robot.setActivelyShooting(true); + } + + double pitchChange = 0; + if (joystick.getRawButton(13)){ + pitchChange = 0.02; + } + else if (joystick.getRawButton(14)){ + pitchChange = -0.02; + } + else{ + pitchChange = 0; + } + double newPos = robot.getTurretPitchPosition() + pitchChange; + if(newPos < 0) newPos = 0; + if(newPos > 1) newPos = 1; + + robot.setTurretPitchPosition(newPos); + //Collector indexer logic based on cargo already in sensors (from jack) double defCollectorPower = 1; diff --git a/src/main/java/frc/robot/generic/Lightning.java b/src/main/java/frc/robot/generic/Lightning.java index b1f4a73..b2a9ff8 100644 --- a/src/main/java/frc/robot/generic/Lightning.java +++ b/src/main/java/frc/robot/generic/Lightning.java @@ -28,7 +28,7 @@ public class Lightning implements GenericRobot { //TODO: update servo ports //servo left was initially set to channel 9, don't know if that means anything - Servo elevationLeft = new Servo(0); + Servo elevationLeft = new Servo(9); Servo elevationRight = new Servo(1); @@ -83,6 +83,8 @@ public Lightning(){ shooterA.setInverted(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); + shootReadyTimer = System.currentTimeMillis(); @@ -269,8 +271,8 @@ public void setTurretPitchPosition(double position){ elevationRight.setBounds(2.0, 1.8, 1.5, 1.2, 1.0); //TODO: figure out use setSpeed() or set() - elevationLeft.setSpeed(position); - elevationRight.setSpeed(position); + elevationLeft.set(position); + elevationRight.set(position); } From 2fc3ca5fe291c090149869ebc9334e3c556f4623 Mon Sep 17 00:00:00 2001 From: BasedGoat123 Date: Thu, 24 Feb 2022 18:24:55 -0500 Subject: [PATCH 25/72] Auto Return C --- src/main/java/frc/robot/Robot.java | 5 +- .../frc/robot/autonomous/BallCtoTerminal.java | 17 +- .../autonomous/BallCtoTerminalReturn.java | 231 ++++++++++++++++++ .../robot/autonomous/GenericAutonomous.java | 2 +- 4 files changed, 251 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cd1ef8c..c505337 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -25,7 +25,8 @@ public class Robot extends TimedRobot { autoArc = new autoArc(), simpleATerminal = new BallAtoTerminal(), simpleBTerminal = new BallBtoTerminal(), - simpleCTerminal = new BallCtoTerminal(); + simpleCTerminal = new BallCtoTerminal(), + CTerminalReturn = new BallCtoTerminalReturn(); GenericRobot robot = new TurretBot(); Joystick joystick = new Joystick(0); @@ -140,6 +141,7 @@ 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()); } @@ -296,6 +298,7 @@ else if (joystick.getRawButton(5)){ if (joystick.getRawButton(5)) autonomous = simpleATerminal; if (joystick.getRawButton(6)) autonomous = simpleBTerminal; if (joystick.getRawButton(7)) autonomous = simpleCTerminal; + if (joystick.getRawButton(9)) autonomous = CTerminalReturn; } diff --git a/src/main/java/frc/robot/autonomous/BallCtoTerminal.java b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java index 7ec04dd..3ed5297 100644 --- a/src/main/java/frc/robot/autonomous/BallCtoTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java @@ -39,6 +39,8 @@ public class BallCtoTerminal extends GenericAutonomous { //TurretTracker tracker = new TurretTracker(); + + boolean initialTurretSpin = true; @Override public void autonomousInit(GenericRobot robot) { autonomousStep = 0; @@ -50,6 +52,7 @@ public void autonomousInit(GenericRobot robot) { PIDTurret = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD()); //tracker.turretInit(robot); + } @Override @@ -66,6 +69,10 @@ public void autonomousPeriodic(GenericRobot robot) { turretv = tv.getDouble(0.0); //tracker.turretUpdate(robot); + + + + switch(autonomousStep){ case 0: //reset robot.lowerCollector(); @@ -108,7 +115,8 @@ public void autonomousPeriodic(GenericRobot robot) { autonomousStep = 12; time = false; break; - case 6: //collector to collect ball + case 6: //turret turn + case 7: //collection part 2 not electric nor boogaloo case 8: //another collection case case 9: //shoot the second ball for funsies @@ -169,6 +177,8 @@ 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++; @@ -187,8 +197,11 @@ public void autonomousPeriodic(GenericRobot robot) { }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..93d463e --- /dev/null +++ b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.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 BallCtoTerminalReturn 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; + + 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; + + //TurretTracker tracker = new TurretTracker(); + + + boolean initialTurretSpin = true; + @Override + public void autonomousInit(GenericRobot robot) { + autonomousStep = 0; + startingYaw = robot.getYaw(); + startTime = System.currentTimeMillis(); + + 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); + + + + + + 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 = 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; + time = false; + break; + case 6: //turret turn + + case 7: //collection part 2 not electric nor boogaloo + case 8: //another 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 + PIDDriveStraight.reset(); + PIDDriveStraight.enableContinuousInput(-180,180); + startDistance = robot.getDriveDistanceInchesLeft(); + autonomousStep +=1; + break; + case 13: //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 14: //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, .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 15: + leftpower = 0; + rightpower = 0; + autonomousStep += 1; + break; + case 16: + 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 17: + leftpower = 0; + rightpower = 0; + 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 = 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/GenericAutonomous.java b/src/main/java/frc/robot/autonomous/GenericAutonomous.java index 15c93ce..c98521a 100644 --- a/src/main/java/frc/robot/autonomous/GenericAutonomous.java +++ b/src/main/java/frc/robot/autonomous/GenericAutonomous.java @@ -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 From 2bce69572aaeb3553b28d9dea77655dbfccd21e7 Mon Sep 17 00:00:00 2001 From: Malti Date: Thu, 24 Feb 2022 18:42:11 -0500 Subject: [PATCH 26/72] edits on align. waiting for testing. --- .../java/frc/robot/autonomous/autoArc.java | 17 +- src/main/java/frc/robot/command/Hang.java | 34 +- .../frc/robot/command/HangWithoutAlign.java | 354 ++++++++++++++++++ 3 files changed, 389 insertions(+), 16 deletions(-) create mode 100644 src/main/java/frc/robot/command/HangWithoutAlign.java diff --git a/src/main/java/frc/robot/autonomous/autoArc.java b/src/main/java/frc/robot/autonomous/autoArc.java index 01f305e..64d3ef4 100644 --- a/src/main/java/frc/robot/autonomous/autoArc.java +++ b/src/main/java/frc/robot/autonomous/autoArc.java @@ -35,6 +35,8 @@ public class autoArc extends GenericAutonomous { double currentTurretPower = 0; double average; int averageTurretXSize = 2; + boolean breakRobot = false; + boolean startTimer = false; @Override public void autonomousInit(GenericRobot robot) { @@ -125,8 +127,19 @@ public void autonomousPeriodic(GenericRobot robot) { currentYaw = robot.getYaw(); correction = PIDPivot.calculate(pivotDeg + currentYaw - startYaw); leftPower = correction; - rightPower = -correction; - currentTurretPower = .05; + rightPower = -correction; //TODO: on lightning, change to abs encoder + if (!robot.isTargetFound() && !breakRobot) { + if (!startTimer) { + startingTime = System.currentTimeMillis(); + startTimer = true; + } + currentTurretPower = .05; + } + if (System.currentTimeMillis() - startingTime > 1000 && !breakRobot){ + currentTurretPower = 0; + breakRobot = true; + } + if (Math.abs(Math.abs(currentYaw - startYaw)-pivotDeg) <= 1.5){ if (!time){ startingTime = System.currentTimeMillis(); diff --git a/src/main/java/frc/robot/command/Hang.java b/src/main/java/frc/robot/command/Hang.java index eda43ad..c7ab280 100644 --- a/src/main/java/frc/robot/command/Hang.java +++ b/src/main/java/frc/robot/command/Hang.java @@ -27,10 +27,9 @@ public class Hang extends GenericCommand{ boolean leftSensor = false; boolean rightSensor = false; - double outerDistArc; double lTraveled; - double fwd = 48; + double fwd = 71.6; PIDController PIDSteering; boolean tapeAlign; @@ -38,10 +37,7 @@ public class Hang extends GenericCommand{ //////////////Now the real stuff - double desiredHeight; - double lowHeight; double escapeHeight = 10;///TODO:what is this?? - double getToBarHeight; boolean firstTime = true; int countLeft = 0; int countRight = 0; @@ -79,6 +75,8 @@ public void step(GenericRobot robot){ SmartDashboard.putBoolean("rightClimberSensor", robot.getClimbSensorRight()); SmartDashboard.putNumber("countLeft", countLeft); SmartDashboard.putNumber("countRight", countRight); + SmartDashboard.putNumber("startDistance", startDistance); + if (tapeAlign) { @@ -93,9 +91,19 @@ public void step(GenericRobot robot){ break; case 0: startAngle = robot.getYaw(); + startDistance = robot.getDriveDistanceInchesLeft(); PIDSteering.reset(); PIDSteering.enableContinuousInput(-180, 180); - commandStep += 1; + commandStep = 12;//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); @@ -120,17 +128,15 @@ public void step(GenericRobot robot){ if (!rightSensor && !robot.getFloorSensorRight()) { differenceDistance = Math.abs(robot.getDriveDistanceInchesLeft() - startDistance); Tapetheta = Math.atan(differenceDistance / sensorDist) * 180 / Math.PI; - outerDistArc = robot.getDriveDistanceInchesRight(); - commandStep = 4; + commandStep += 1; } else if (!leftSensor && !robot.getFloorSensorLeft()) { differenceDistance = Math.abs(robot.getDriveDistanceInchesLeft() - startDistance); Tapetheta = Math.atan(differenceDistance / sensorDist) * 180 / Math.PI; - outerDistArc = robot.getDriveDistanceInchesLeft(); - commandStep = 4; + commandStep += 1; } break; - case 4: + case 3: if (leftSensor) { currentYaw = startAngle - Tapetheta; //currentYaw = targetYaw because we are lazy } else { @@ -141,17 +147,17 @@ public void step(GenericRobot robot){ startDistance = robot.getDriveDistanceInchesLeft(); commandStep += 1; break; - case 5: + case 4: correction = PIDSteering.calculate(robot.getYaw() - currentYaw); leftPower = defaultPower + correction; rightPower = defaultPower - correction; - if (Math.abs(robot.getDriveDistanceInchesLeft() - startDistance) >= (fwd - lTraveled)) { + if (Math.abs(robot.getDriveDistanceInchesLeft() - startDistance) >= (fwd)) { leftPower = 0; rightPower = 0; commandStep += 1; } break; - case 6: //adios amigos + case 5: //adios amigos leftPower = 0; rightPower = 0; //tapeAlign = false; TODO: make this stop being a comment 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..331521b --- /dev/null +++ b/src/main/java/frc/robot/command/HangWithoutAlign.java @@ -0,0 +1,354 @@ +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 = 12.0; + double Tapetheta = 0; + + double correction; + double currentYaw; + + long startingTime = 0; + + boolean leftSensor = false; + boolean rightSensor = false; + + double lTraveled; + + double fwd = 71.6; + 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 = -.5; + double defaultClimbPowerDown = .5; + boolean leftArrived = false; + boolean rightArrived = false; + double startHeightLeft = 0; + double startHeightRight = 0; + + + public void begin(GenericRobot robot){ + startingTime = System.currentTimeMillis(); + commandStep = -1; + leftSensor = false; + rightSensor = false; + lTraveled = 0; + fwd = 71.6; + PIDSteering = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD()); + 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.encoderTicksLeftDrive()); + SmartDashboard.putNumber("rightEncoderRaw", robot.encoderTicksRightDrive()); + 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); + + + //////////////////////////start the real stuff now +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + switch (commandStep){ + case -1: + commandStep += 1; + break; + case 0:///reset and enable PTO + //reset encoders + robot.turnOnPTO(); + robot.resetEncoders(); + countLeft = 0; + countRight = 0; + if (System.currentTimeMillis() - startingTime >= 50){ + commandStep += 1; + } + + break; + case 1: ///////////unlock rotation piston to send arms forward + robot.setArmsForward(); + 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; + commandStep += 1; + + } + + break; + case 3: ///////////unlock rotation piston to send arms forward + robot.setArmsBackward(); + 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; + commandStep = 30; + leftArrived = false; + rightArrived = false; + startHeightLeft = robot.armHeightLeft(); + startHeightRight = robot.armHeightRight(); + startingTime = System.currentTimeMillis(); + + } + break; + case 30: //delay :)TODO:change + if (System.currentTimeMillis() - startingTime >= 5000){ + commandStep = 12; + } + 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 + } + 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(); + commandStep += 1; + 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; + } + + 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; + + + } + robot.armPower(leftArmPower, rightArmPower); + } + +} From 6757694391ae16fa2f8701212b366b7b7b2e5224 Mon Sep 17 00:00:00 2001 From: Malti Date: Thu, 24 Feb 2022 22:49:18 -0500 Subject: [PATCH 27/72] Added in autoshooting and collecting methods. Implemented into auto routines. --- .../frc/robot/autonomous/BallBtoTerminal.java | 86 ++++++-- .../autonomous/BallBtoTerminalReturn.java | 200 ++++++++++++++++++ .../frc/robot/autonomous/BallCtoTerminal.java | 60 ++++-- .../autonomous/BallCtoTerminalReturn.java | 88 +++++--- .../java/frc/robot/autonomous/autoArc.java | 98 ++++++--- .../java/frc/robot/generic/GenericRobot.java | 13 ++ .../java/frc/robot/generic/Lightning.java | 52 ++++- 7 files changed, 498 insertions(+), 99 deletions(-) create mode 100644 src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java diff --git a/src/main/java/frc/robot/autonomous/BallBtoTerminal.java b/src/main/java/frc/robot/autonomous/BallBtoTerminal.java index ec5e21e..245892e 100644 --- a/src/main/java/frc/robot/autonomous/BallBtoTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallBtoTerminal.java @@ -20,6 +20,12 @@ public class BallBtoTerminal extends GenericAutonomous { double distanceTerminal = 160.6; double rampDownDist = 10; + PIDController PIDTurret; + + int averageTurretXSize = 2; + double[] averageTurretX = new double [averageTurretXSize]; + + int counter = 0; PIDController PIDDriveStraight; @@ -29,11 +35,15 @@ public void autonomousInit(GenericRobot robot) { 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) { + robot.getCargo(); + robot.shoot(); + robot.setTurretPitchPosition(.38); switch(autonomousStep){ case 0: //reset robot.lowerCollector(); @@ -48,9 +58,19 @@ public void autonomousPeriodic(GenericRobot robot) { } break; case 1: //shoot the ball + if (robot.canShoot()){ + robot.setActivelyShooting(true); + startTime = System.currentTimeMillis(); + autonomousStep += 1; + } + break; case 2: //shoot the ball part 2 electric boogaloo - case 3: //shoot the ball part 3 maybe - case 4: //drive to ball B + if (System.currentTimeMillis() - startTime >= 250){ + robot.setActivelyShooting(false); + autonomousStep += 1; + } + break; + case 3: //drive to ball B correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); leftpower = defaultPower + correction; @@ -67,19 +87,26 @@ public void autonomousPeriodic(GenericRobot robot) { startTime = System.currentTimeMillis(); } break; - case 5: //stop + case 4: //stop leftpower = 0; rightpower = 0; - autonomousStep = 12; + autonomousStep += 1; + break; + case 5: + if (robot.canShoot()){ + robot.setActivelyShooting(true); + startTime = System.currentTimeMillis(); + autonomousStep += 1.0; + } + break; + case 6: // part 2 not electric nor boogaloo + if (System.currentTimeMillis() - startTime >= 500){ + robot.setActivelyShooting(false); + autonomousStep += 1; + } 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 + + case 7://reset if (System.currentTimeMillis() - startTime >= 1000){ PIDDriveStraight.reset(); PIDDriveStraight.enableContinuousInput(-180,180); @@ -87,14 +114,16 @@ public void autonomousPeriodic(GenericRobot robot) { autonomousStep +=1; } break; - case 13://drive to ball at terminal + case 8://drive to ball at 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; + double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist, robot.getDriveDistanceInchesLeft(), distanceTerminal); + leftpower = ramp; + rightpower = ramp; } if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal) { autonomousStep += 1; @@ -102,16 +131,35 @@ public void autonomousPeriodic(GenericRobot robot) { rightpower = 0; } break; - case 14: + case 9: 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); + 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..bdf5e06 --- /dev/null +++ b/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java @@ -0,0 +1,200 @@ +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 = .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) { + + 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 = 4; + } + break; + case 1: //shoot the ball + if (robot.canShoot()){ + robot.setActivelyShooting(true); + startTime = System.currentTimeMillis(); + autonomousStep += 1; + } + break; + case 2: //shoot the ball part 2 electric boogaloo + if (System.currentTimeMillis() - startTime >= 250){ + robot.setActivelyShooting(false); + autonomousStep += 1; + } + break; + case 3: //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 4: //stop + leftpower = 0; + rightpower = 0; + autonomousStep += 1; + break; + case 5: + if (robot.canShoot()){ + robot.setActivelyShooting(true); + startTime = System.currentTimeMillis(); + autonomousStep += 1.0; + } + break; + case 6: // part 2 not electric nor boogaloo + if (System.currentTimeMillis() - startTime >= 500){ + robot.setActivelyShooting(false); + autonomousStep += 1; + } + break; + + case 7://reset + if (System.currentTimeMillis() - startTime >= 1000){ + PIDDriveStraight.reset(); + PIDDriveStraight.enableContinuousInput(-180,180); + startDistance = robot.getDriveDistanceInchesLeft(); + autonomousStep +=1; + } + break; + case 8://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 9: + leftpower = 0; + rightpower = 0; + autonomousStep += 1.0; + break; + + case 10: + 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 11: + if (robot.canShoot()){ + robot.setActivelyShooting(true); + startTime = System.currentTimeMillis(); + autonomousStep += 1.00; + } + break; + case 12: + if (System.currentTimeMillis() - startTime >= 251){ + robot.setActivelyShooting(false); + autonomousStep += 1; + } + case 13: + 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/BallCtoTerminal.java b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java index 3ed5297..97e083a 100644 --- a/src/main/java/frc/robot/autonomous/BallCtoTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java @@ -25,6 +25,8 @@ public class BallCtoTerminal extends GenericAutonomous { double angleC = 82.74; double rampDownDist = 10; + double shooterTargetRPM; + PIDController PIDDriveStraight; PIDController PIDTurret; PIDController PIDPivot; @@ -37,6 +39,8 @@ public class BallCtoTerminal extends GenericAutonomous { double turretv; int counter = 0; + boolean readyToShoot = false; + //TurretTracker tracker = new TurretTracker(); @@ -46,7 +50,8 @@ 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()); @@ -72,7 +77,9 @@ public void autonomousPeriodic(GenericRobot robot) { - + robot.getCargo(); + robot.shoot(); + robot.setTurretPitchPosition(.38); switch(autonomousStep){ case 0: //reset robot.lowerCollector(); @@ -83,15 +90,25 @@ public void autonomousPeriodic(GenericRobot robot) { robot.resetEncoders(); robot.resetAttitude(); if (System.currentTimeMillis() - startTime > 100){ - autonomousStep = 4; + autonomousStep += 1; 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 + 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 A correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); leftpower = defaultPower + correction; @@ -108,7 +125,7 @@ public void autonomousPeriodic(GenericRobot robot) { startTime = System.currentTimeMillis(); } break; - case 5: //stop + case 4: //stop leftpower = 0; rightpower = 0; startDistance = robot.getDriveDistanceInchesLeft(); @@ -116,20 +133,24 @@ public void autonomousPeriodic(GenericRobot robot) { time = false; break; case 6: //turret turn - - case 7: //collection part 2 not electric nor boogaloo - case 8: //another 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 (robot.canShoot()){ + robot.setActivelyShooting(true); + startTime = System.currentTimeMillis(); + autonomousStep += 1.0; + } + break; + case 7: + if (System.currentTimeMillis() - startTime >= 500){ + robot.setActivelyShooting(false); + autonomousStep += 1; + } + case 8://reset PIDDriveStraight.reset(); PIDDriveStraight.enableContinuousInput(-180,180); startDistance = robot.getDriveDistanceInchesLeft(); autonomousStep +=1; break; - case 13: //turn to go to ball @ terminal + case 9: //turn to go to ball @ terminal correction = PIDPivot.calculate(angleC + robot.getYaw() - startingYaw ); leftpower = correction; rightpower = -correction; @@ -153,7 +174,7 @@ public void autonomousPeriodic(GenericRobot robot) { startTime = System.currentTimeMillis(); } break; - case 14: //drive towards the ball + case 10: //drive towards the ball correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); leftpower = defaultPower + correction; @@ -170,12 +191,13 @@ public void autonomousPeriodic(GenericRobot robot) { startTime = System.currentTimeMillis(); } break; - case 15: + case 11: leftpower = 0; rightpower = 0; break; } robot.drivePercent(leftpower, rightpower); + robot.setShooterRPM(shooterTargetRPM, shooterTargetRPM); //If turret works set value of averageTurretX[] to turretx diff --git a/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java index 93d463e..23f227d 100644 --- a/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java +++ b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java @@ -37,6 +37,12 @@ public class BallCtoTerminalReturn extends GenericAutonomous { double turretv; int counter = 0; + double shooterTargetRPM; + boolean readyToShoot; + + double indexerPct; + double collectorPct; + //TurretTracker tracker = new TurretTracker(); @@ -46,7 +52,10 @@ 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()); @@ -72,7 +81,8 @@ public void autonomousPeriodic(GenericRobot robot) { - + robot.getCargo(); + robot.shoot(); switch(autonomousStep){ case 0: //reset robot.lowerCollector(); @@ -83,15 +93,26 @@ public void autonomousPeriodic(GenericRobot robot) { robot.resetEncoders(); robot.resetAttitude(); if (System.currentTimeMillis() - startTime > 100){ - autonomousStep = 4; + autonomousStep += 1; 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 + 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: //turn the shooter off + if (System.currentTimeMillis() - startTime >= 250){ + robot.setActivelyShooting(false); + autonomousStep += 1; + } + break; + case 3: //drive to ball C + collectorPct = 1; correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); leftpower = defaultPower + correction; @@ -108,28 +129,34 @@ public void autonomousPeriodic(GenericRobot robot) { startTime = System.currentTimeMillis(); } break; - case 5: //stop + case 4: //stop leftpower = 0; rightpower = 0; startDistance = robot.getDriveDistanceInchesLeft(); autonomousStep = 12; time = false; break; - case 6: //turret turn - - case 7: //collection part 2 not electric nor boogaloo - case 8: //another 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 + case 5: //collect cargo + if (robot.canShoot()){ + robot.setActivelyShooting(true); + startTime = System.currentTimeMillis(); + autonomousStep += 1.0; + } + break; + case 6: //sets shooter's RPM + if (System.currentTimeMillis() - startTime >= 500){ + robot.setActivelyShooting(false); + autonomousStep += 1; + } + break; + + case 7://reset PIDDriveStraight.reset(); PIDDriveStraight.enableContinuousInput(-180,180); startDistance = robot.getDriveDistanceInchesLeft(); autonomousStep +=1; break; - case 13: //turn to go to ball @ terminal + case 8: //turn to go to Ball Terminal correction = PIDPivot.calculate(angleC + robot.getYaw() - startingYaw ); leftpower = correction; rightpower = -correction; @@ -153,7 +180,8 @@ public void autonomousPeriodic(GenericRobot robot) { startTime = System.currentTimeMillis(); } break; - case 14: //drive towards the ball + case 9: //drive towards Ball Terminal + correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); leftpower = defaultPower + correction; @@ -171,12 +199,12 @@ public void autonomousPeriodic(GenericRobot robot) { startTime = System.currentTimeMillis(); } break; - case 15: + case 10: //collect Ball Terminal leftpower = 0; rightpower = 0; autonomousStep += 1; break; - case 16: + case 11: //Drive forward a better shooting position correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); leftpower = -1*(defaultPower - correction); @@ -194,15 +222,27 @@ public void autonomousPeriodic(GenericRobot robot) { autonomousStep += 1; } break; - case 17: + case 12: //shoot it :) + if (robot.canShoot()){ + robot.setActivelyShooting(true); + startTime = System.currentTimeMillis(); + autonomousStep += 1.00; + } + break; + case 13: //shoot part 2 + if (System.currentTimeMillis() - startTime >= 250){ + robot.setActivelyShooting(false); + autonomousStep += 1.0; + } + break; + case 14: //End of autonomous, wait for Teleop leftpower = 0; rightpower = 0; break; } robot.drivePercent(leftpower, rightpower); - //If turret works set value of averageTurretX[] to turretx - + //If turret works set value of averageTurretX[] to turretx if(turretv !=0 ) { averageTurretX[counter % averageTurretXSize] = turretx; counter++; diff --git a/src/main/java/frc/robot/autonomous/autoArc.java b/src/main/java/frc/robot/autonomous/autoArc.java index 9dd1c68..23234ba 100644 --- a/src/main/java/frc/robot/autonomous/autoArc.java +++ b/src/main/java/frc/robot/autonomous/autoArc.java @@ -62,30 +62,10 @@ public void autonomousInit(GenericRobot robot) { @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 + robot.getCargo(); + robot.shoot(); + robot.setTurretPitchPosition(.38); switch (autonomousStep){ case 0: //reset robot.lowerCollector(); @@ -101,8 +81,20 @@ public void autonomousPeriodic(GenericRobot robot) { autonomousStep += 1; } break; - - case 1: //straightaway + case 1: + if (robot.canShoot()){ + startingTime = System.currentTimeMillis(); + robot.setActivelyShooting(true); + autonomousStep += 1; + } + break; + case 2: + if (System.currentTimeMillis() - startingTime >= 250){ + robot.setActivelyShooting(false); + autonomousStep += 1; + } + break; + case 3: //straightaway currentYaw = robot.getYaw(); currentDistInches = robot.getDriveDistanceInchesLeft(); correction = PIDSteering.calculate(currentYaw - startYaw); @@ -120,8 +112,21 @@ public void autonomousPeriodic(GenericRobot robot) { autonomousStep += 1; } break; - - case 2: // Pid pivot 90 degrees ccw + case 4: + if (robot.canShoot()){ + startingTime = System.currentTimeMillis(); + robot.setActivelyShooting(true); + autonomousStep += 1.0; + } + break; + case 5: + if (System.currentTimeMillis() - startingTime >= 500){ + robot.setActivelyShooting(true); + robot.setShooterTargetRPM(500); //super low power to yeet out undesirable cargo + autonomousStep += 1; + } + break; + case 6: // Pid pivot 90 degrees ccw currentYaw = robot.getYaw(); correction = PIDPivot.calculate(pivotDeg + currentYaw - startYaw); leftPower = correction; @@ -145,7 +150,7 @@ public void autonomousPeriodic(GenericRobot robot) { } break; - case 3: //Pid reset + case 7: //Pid reset PIDSteering.reset(); PIDPivot.reset(); PIDSteering.disableContinuousInput(); @@ -155,7 +160,7 @@ public void autonomousPeriodic(GenericRobot robot) { autonomousStep += 1; break; - case 4: //Pid Arc 10 ft Left + case 8: //Pid Arc 10 ft Left currentYaw = robot.getYaw(); currentDistInches = robot.getDriveDistanceInchesRight(); correction = PIDSteering.calculate(Math.toDegrees((currentDistInches-startInches)/outerRadius) + (currentYaw-startYaw)); @@ -167,15 +172,48 @@ public void autonomousPeriodic(GenericRobot robot) { rightPower = 0; autonomousStep += 1; } + if (currentDistInches - startInches >= outerArcDist - 24){ + robot.setShooterTargetRPM(5000); + robot.setActivelyShooting(false); + } break; - case 5: //stop + case 9: //stop leftPower = 0; rightPower = 0; + autonomousStep += 1; break; + case 10: //shoot last cargo + if (robot.canShoot()){ + robot.setActivelyShooting(true); + } + break; + } robot.drivePercent(leftPower, rightPower); + + if(robot.isTargetFound()) { + averageX[counter % averageTurretXSize] = robot.getTargetX(); + counter++; + } + + double average = 0; + for(double i: averageX){ + average += i; + } + average /= averageTurretXSize; + + double currentTurretPower = 0; + + if(robot.isTargetFound()){ + currentTurretPower = turretPIDController.calculate(average); + }else{ + turretPIDController.reset(); + } + if((!robot.isTargetFound()) && (System.currentTimeMillis() - startingTime < 2000)) { + currentTurretPower = .2; + } robot.setTurretPowerPct(currentTurretPower); } diff --git a/src/main/java/frc/robot/generic/GenericRobot.java b/src/main/java/frc/robot/generic/GenericRobot.java index 6193e4c..051b3b7 100644 --- a/src/main/java/frc/robot/generic/GenericRobot.java +++ b/src/main/java/frc/robot/generic/GenericRobot.java @@ -114,6 +114,17 @@ public enum TeamColor { 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; @@ -261,6 +272,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"); } diff --git a/src/main/java/frc/robot/generic/Lightning.java b/src/main/java/frc/robot/generic/Lightning.java index b2a9ff8..e14cafa 100644 --- a/src/main/java/frc/robot/generic/Lightning.java +++ b/src/main/java/frc/robot/generic/Lightning.java @@ -10,8 +10,8 @@ public class Lightning implements GenericRobot { public static final double TICKS_PER_INCH_DRIVE = 0.96; 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; AHRS navx = new AHRS(SPI.Port.kMXP, (byte) 50); @@ -59,12 +59,13 @@ public class Lightning implements GenericRobot { SparkMaxLimitSwitch limitSwitchLeftBForward = leftMotorB.getForwardLimitSwitch(lstype); SparkMaxLimitSwitch limitSwitchLeftBReverse = leftMotorB.getReverseLimitSwitch(lstype); - double defaultShooterTargetRPM = 500; + double defaultShooterTargetRPM = 5000; 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; @@ -176,6 +177,42 @@ 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; + if (getShooterRPMTop() >= shooterRPM && getShooterRPMBottom() >= shooterRPM){ + canShoot = true; + } + else{ + canShoot = false; + } + } + + @Override + public boolean canShoot(){ + return canShoot; + } + @Override public void setCollectorIntakePercentage(double percentage) { @@ -256,8 +293,6 @@ public double getTurretPowerPct() { @Override public double getTurretPitchPosition(){ - elevationLeft.setBounds(2.0, 1.8, 1.5, 1.2, 1.0); - elevationRight.setBounds(2.0, 1.8, 1.5, 1.2, 1.0); //TODO: getSpeed()? getPosition()? getAngle()? don't know which to use return elevationLeft.getSpeed(); @@ -267,8 +302,6 @@ public void setTurretPitchPosition(double position){ if(position < 0) position = 0; if(position > 1) position = 1; - elevationLeft.setBounds(2.0, 1.8, 1.5, 1.2, 1.0); - elevationRight.setBounds(2.0, 1.8, 1.5, 1.2, 1.0); //TODO: figure out use setSpeed() or set() elevationLeft.set(position); @@ -354,6 +387,11 @@ public double getShooterTargetRPM(){ return defaultShooterTargetRPM; } + @Override + public void setShooterTargetRPM(double rpm){ + defaultShooterTargetRPM = rpm; + } + @Override public boolean getFloorSensorLeft(){ return limitSwitchLeftBForward.isPressed(); From b887fafe72aa7fa91bf1550fede46120218beab7 Mon Sep 17 00:00:00 2001 From: Malti Date: Thu, 24 Feb 2022 22:51:52 -0500 Subject: [PATCH 28/72] minor edits to climb code --- src/main/java/frc/robot/command/Hang.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/command/Hang.java b/src/main/java/frc/robot/command/Hang.java index c7ab280..36cc0ea 100644 --- a/src/main/java/frc/robot/command/Hang.java +++ b/src/main/java/frc/robot/command/Hang.java @@ -33,7 +33,7 @@ public class Hang extends GenericCommand{ PIDController PIDSteering; boolean tapeAlign; - /////^^^^^^^^^^^Stuff for tapeAlign + /////^^^^^^^^^^^Stuff for tapeAlign/ //////////////Now the real stuff From deeaf365a1b5d6513acc88fb7ed8211b22a0ec04 Mon Sep 17 00:00:00 2001 From: JW314 Date: Thu, 24 Feb 2022 23:32:23 -0500 Subject: [PATCH 29/72] Code used for Lightning shooter testing on 2/24 --- src/main/java/frc/robot/Robot.java | 22 ++++++++---- .../java/frc/robot/generic/Lightning.java | 35 +++++++++++++------ 2 files changed, 41 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 92ce2e2..7b894f8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -45,6 +45,8 @@ public class Robot extends TimedRobot { double currentTurretPower; + + PIDController turretPIDController; @@ -153,7 +155,6 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { turretPIDController = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD()); - } @Override public void teleopPeriodic() { @@ -169,12 +170,13 @@ public class Robot extends TimedRobot { double scaleFactor = 1.0; //robot PTO not on arms, give joystick carte blanche + /** if(!robot.getPTOState()){ robot.drivePercent( (jy+jx) * scaleFactor, (jy-jx) * scaleFactor ); - } + }**/ @@ -259,19 +261,18 @@ else if(xbox.getRawAxis(rightAxis) < -tolerance){ robot.setActivelyShooting(true); } + double pitchChange = 0; - if (joystick.getRawButton(13)){ + if (joystick.getRawButtonPressed(13)){ pitchChange = 0.02; } - else if (joystick.getRawButton(14)){ + else if (joystick.getRawButtonPressed(14)){ pitchChange = -0.02; } else{ pitchChange = 0; } double newPos = robot.getTurretPitchPosition() + pitchChange; - if(newPos < 0) newPos = 0; - if(newPos > 1) newPos = 1; robot.setTurretPitchPosition(newPos); @@ -297,6 +298,9 @@ else if (joystick.getRawButton(14)){ curCollector = 0; } } + if(robot.isActivelyShooting()){ + curIndexer = defIndexerPower; + } } else if (joystick.getRawButton(5)){ curCollector = -defCollectorPower; @@ -315,6 +319,8 @@ else if (joystick.getRawButton(5)){ robot.setCollectorIntakePercentage(curCollector); robot.setIndexerIntakePercentage(curIndexer); + //robot.drivePercent(0, 0); + } @Override public void disabledInit() {} @@ -410,5 +416,9 @@ else if (joystick.getRawButton(14)){ if (joystick.getRawButton( 5)) robot.setArmsForward(); if (joystick.getRawButton(10)) robot.setArmsBackward(); + + //robot.drivePercent(0, 0); + + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/generic/Lightning.java b/src/main/java/frc/robot/generic/Lightning.java index b2a9ff8..af611d7 100644 --- a/src/main/java/frc/robot/generic/Lightning.java +++ b/src/main/java/frc/robot/generic/Lightning.java @@ -10,8 +10,8 @@ public class Lightning implements GenericRobot { public static final double TICKS_PER_INCH_DRIVE = 0.96; 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; AHRS navx = new AHRS(SPI.Port.kMXP, (byte) 50); @@ -59,7 +59,7 @@ public class Lightning implements GenericRobot { SparkMaxLimitSwitch limitSwitchLeftBForward = leftMotorB.getForwardLimitSwitch(lstype); SparkMaxLimitSwitch limitSwitchLeftBReverse = leftMotorB.getReverseLimitSwitch(lstype); - double defaultShooterTargetRPM = 500; + double defaultShooterTargetRPM = 5000; boolean isPTOonArms; @@ -85,6 +85,19 @@ public Lightning(){ 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(2.5e-4); + shooterAPIDController.setI(0); + shooterAPIDController.setD(2.5e-1); + shooterAPIDController.setFF(1.67e-4); + shooterAPIDController.getIZone(500); + shooterAPIDController.getDFilter(0); + + shooterBPIDController.setP(2.5e-4); + shooterBPIDController.setI(0); + shooterBPIDController.setD(2.5e-1); + shooterBPIDController.setFF(1.67e-4); + shooterBPIDController.getIZone(500); + shooterBPIDController.getDFilter(0); shootReadyTimer = System.currentTimeMillis(); @@ -93,6 +106,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); @@ -101,6 +117,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 @@ -256,20 +276,15 @@ public double getTurretPowerPct() { @Override public double getTurretPitchPosition(){ - elevationLeft.setBounds(2.0, 1.8, 1.5, 1.2, 1.0); - elevationRight.setBounds(2.0, 1.8, 1.5, 1.2, 1.0); //TODO: getSpeed()? getPosition()? getAngle()? don't know which to use - return elevationLeft.getSpeed(); + return elevationLeft.get(); } @Override public void setTurretPitchPosition(double position){ if(position < 0) position = 0; if(position > 1) position = 1; - elevationLeft.setBounds(2.0, 1.8, 1.5, 1.2, 1.0); - elevationRight.setBounds(2.0, 1.8, 1.5, 1.2, 1.0); - //TODO: figure out use setSpeed() or set() elevationLeft.set(position); elevationRight.set(position); @@ -322,7 +337,7 @@ public void setShooterRPMTop(double rpm) { @Override public void setShooterRPMBottom(double rpm) { - shooterBPIDController.setReference(rpm, CANSparkMax.ControlType.kVelocity); + shooterBPIDController.setReference(rpm, CANSparkMax.ControlType.kVelocity);// } From 8c9cca6f023d09d480adca2aabae9dc14ec35eac Mon Sep 17 00:00:00 2001 From: Malti Date: Fri, 25 Feb 2022 19:47:20 -0500 Subject: [PATCH 30/72] Merged simpleAuto and full_shooter into hang. --- src/main/java/frc/robot/Robot.java | 171 +++++++++++++---------------- 1 file changed, 78 insertions(+), 93 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d177283..37d3736 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -210,6 +210,7 @@ public class Robot extends TimedRobot { if (joystick.getRawButtonPressed(8)) { count = (count + 1) % 2; } + if (count == 1) { hang = true; } else { @@ -219,26 +220,24 @@ public class Robot extends TimedRobot { if (!hang) { reset = true; + //////////////////////////////////////////////////DRIVETRAIN CONTROL 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; - //moved this to after joystick deaden because deaden should be focused on the raw joystick values double scaleFactor = 1.0; - - //robot PTO not on arms, give joystick carte blanche if (!robot.getPTOState()) { robot.drivePercent( (jy + jx) * scaleFactor, (jy - jx) * scaleFactor ); } + //////////////////////////////////////////////DRIVETRAIN CONTROL ENDS - + ///////////////////////////////////////////////////////////////////////SET UP XBOX 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)); @@ -249,76 +248,44 @@ public class Robot extends TimedRobot { int rightAxis = 5; double tolerance = 0.8; double drivePower = 0.2; + ///////////////////////////////////////////////////////////////////SET UP OVER + + //////////////////////////////////////////////////////////TURRET CONTROL + if (joystick.getRawButton(12)) { + currentTurretPower = .2; + } + else if (joystick.getRawButton(15)) { + currentTurretPower = -.2; + } + else { + currentTurretPower = 0.0; + } + robot.setTurretPowerPct(currentTurretPower); + /////////////////////////////////////////////////////TURRET CONTROL ENDS - if (joystick.getRawButton(12)) robot.setTurretPowerPct(0.2); - else if (joystick.getRawButton(15)) robot.setTurretPowerPct(-0.2); - else robot.setTurretPowerPct(0.0); + /////////////////////////////////////////////////////CLIMBER WHEN PTO 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 (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); - } - } - if (hang) { - ActuallyHanging = true; - SmartDashboard.putBoolean("we really are hanging", ActuallyHanging); - if (reset) { - command.begin(robot); - reset = false; + if (xbox.getRawAxis(rightAxis) > tolerance) { + driveRight = drivePower; + } else if (xbox.getRawAxis(rightAxis) < -tolerance) { + driveRight = -drivePower; } - command.step(robot); - } - - - //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; + robot.drivePercent(driveLeft, driveRight); } - average /= averageTurretXSize; - SmartDashboard.putNumber("Average", average); + /////////////////////////////////////////////////////CLIMBER CODE ENDS - if (joystick.getRawButtonPressed(1)) turretPIDController.reset(); - if (joystick.getRawButton(1) && robot.isTargetFound()) { - currentTurretPower = turretPIDController.calculate(average); - } else { - if (joystick.getRawButton(3)) currentTurretPower = -0.1; - else if (joystick.getRawButton(4)) currentTurretPower = 0.1; - else currentTurretPower = 0.0; - } - if (joystick.getRawButton(12)) currentTurretPower = 0.2; - else if (joystick.getRawButton(15)) currentTurretPower = -0.2; - - robot.setTurretPowerPct(currentTurretPower); + //////////////////////////////////////////////////////////SHOOTER CODE BEGINS double shooterTargetRPM = 0; if (joystick.getRawButton(11)) { shooterTargetRPM = robot.getShooterTargetRPM(); @@ -342,56 +309,59 @@ public class Robot extends TimedRobot { if (joystick.getRawButton(7)) { robot.setActivelyShooting(true); } + ////////////////////////////////////////////////////////////SHOOTER CODE ENDS - double pitchChange = 0; - if (joystick.getRawButtonPressed(13)){ - pitchChange = 0.02; - } - else if (joystick.getRawButtonPressed(14)){ - pitchChange = -0.02; - } - else{ - pitchChange = 0; - } - double newPos = robot.getTurretPitchPosition() + pitchChange; + ///////////////////////////////////////////////////////////ACTUATOR STUFF + double pitchChange = 0; + if (joystick.getRawButtonPressed(13)){ + pitchChange = 0.02; + } + else if (joystick.getRawButtonPressed(14)){ + pitchChange = -0.02; + } + else{ + pitchChange = 0; + } + double newPos = robot.getTurretPitchPosition() + pitchChange; robot.setTurretPitchPosition(newPos); + /////////////////////////////////////////////////////////////////ACTUATOR STUFF ENDS - //Collector indexer logic based on cargo already in sensors (from jack) + /////////////////////////////////////////////////////////COLLECTOR CONTROLS double defCollectorPower = 1; double defIndexerPower = 1; double curCollector; double curIndexer; - //button 2 = bottom center button - if(joystick.getRawButton(2)){ - if(!robot.getUpperCargo()){ - curCollector = defCollectorPower; - curIndexer = defIndexerPower; - } - else{ - curIndexer = 0; - if(!robot.getLowerCargo()){ + //button 2 = bottom center button + if(joystick.getRawButton(2)){ + if(!robot.getUpperCargo()){ curCollector = defCollectorPower; + curIndexer = defIndexerPower; } else{ - curCollector = 0; + curIndexer = 0; + if(!robot.getLowerCargo()){ + curCollector = defCollectorPower; + } + else{ + curCollector = 0; + } + } + if(robot.isActivelyShooting()){ + curIndexer = defIndexerPower; } } - if(robot.isActivelyShooting()){ - curIndexer = defIndexerPower; + else if (joystick.getRawButton(5)){ + curCollector = -defCollectorPower; + curIndexer = -defIndexerPower; + } + else{ + curCollector = 0; + curIndexer = 0; } - } - else if (joystick.getRawButton(5)){ - 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()) { @@ -400,12 +370,27 @@ else if (joystick.getRawButton(5)){ robot.setCollectorIntakePercentage(curCollector); robot.setIndexerIntakePercentage(curIndexer); + //////////////////////////////////////////////////COLLECTOR LOGIC ENDS - //robot.drivePercent(0, 0); } + if (hang) { + ///////////////////////////////////////////RUN AUTO-CLIMB + ActuallyHanging = true; + SmartDashboard.putBoolean("we really are hanging", ActuallyHanging); + if (reset) { + command.begin(robot); + reset = false; + } + command.step(robot); + } + + + + } + @Override public void disabledInit() {} @Override public void disabledPeriodic() { From 1e91503be9b28878048e0f333cfe012742a14b38 Mon Sep 17 00:00:00 2001 From: Louminator Date: Fri, 25 Feb 2022 20:14:42 -0500 Subject: [PATCH 31/72] Added turret controls. --- src/main/java/frc/robot/Robot.java | 48 +++++++++++++++++++----------- 1 file changed, 30 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 37d3736..2d5db7d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -5,9 +5,6 @@ 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; @@ -16,7 +13,6 @@ import frc.robot.command.*; import frc.robot.generic.GenericRobot; import frc.robot.generic.Lightning; -import frc.robot.generic.TurretBot; public class Robot extends TimedRobot { @@ -45,7 +41,7 @@ public class Robot extends TimedRobot { double turretv;*/ int counter = 0; double average; - double currentTurretPower; + double turretPower; boolean hang = false; int count = 0; boolean reset = true; @@ -206,6 +202,8 @@ public class Robot extends TimedRobot { @Override public void teleopPeriodic() { + double turretPower = 0; + //note to self: buttons currently assume mirrored joystick setting if (joystick.getRawButtonPressed(8)) { count = (count + 1) % 2; @@ -251,16 +249,31 @@ public class Robot extends TimedRobot { ///////////////////////////////////////////////////////////////////SET UP OVER //////////////////////////////////////////////////////////TURRET CONTROL - if (joystick.getRawButton(12)) { - currentTurretPower = .2; - } - else if (joystick.getRawButton(15)) { - currentTurretPower = -.2; + + counter = (counter + 1) % averageTurretXSize; + averageX[counter] = robot.getTargetX(); + turretPIDController.calculate(average); + + double average = 0; + for (int i = 0; i < averageTurretXSize; i++) + average += averageX[i]; + average /= averageTurretXSize; + + if ((xbox.getRawAxis(4) > 0.80) & robot.isTargetFound()) + { + turretPower = turretPIDController.calculate(average); } - else { - currentTurretPower = 0.0; + else + { + turretPIDController.reset(); + if (xbox.getRawButton(5)) { + turretPower = -0.45; + } else if (xbox.getRawButton(6)) { + turretPower = 0.45; + } else { + turretPower = 0.0; + } } - robot.setTurretPowerPct(currentTurretPower); /////////////////////////////////////////////////////TURRET CONTROL ENDS @@ -368,10 +381,12 @@ else if (joystick.getRawButton(5)){ curIndexer = defIndexerPower; } - robot.setCollectorIntakePercentage(curCollector); - robot.setIndexerIntakePercentage(curIndexer); + //////////////////////////////////////////////////COLLECTOR LOGIC ENDS + robot.setTurretPowerPct(turretPower); + robot.setCollectorIntakePercentage(curCollector); + robot.setIndexerIntakePercentage(curIndexer); } if (hang) { @@ -385,9 +400,6 @@ else if (joystick.getRawButton(5)){ command.step(robot); } - - - } From 599da9493435428b75d3791bf4e5bc62bad317c3 Mon Sep 17 00:00:00 2001 From: Malti Date: Fri, 25 Feb 2022 20:15:34 -0500 Subject: [PATCH 32/72] changes to drive control code. --- src/main/java/frc/robot/Robot.java | 126 +++++++++++++++-------------- 1 file changed, 64 insertions(+), 62 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 37d3736..d5a2082 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -5,9 +5,6 @@ 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; @@ -16,7 +13,6 @@ import frc.robot.command.*; import frc.robot.generic.GenericRobot; import frc.robot.generic.Lightning; -import frc.robot.generic.TurretBot; public class Robot extends TimedRobot { @@ -39,10 +35,7 @@ public class Robot extends TimedRobot { int averageTurretXSize = 2; double[] averageX = new double [averageTurretXSize]; - /*double turretx; - double turrety; - double turretarea; - double turretv;*/ + int counter = 0; double average; double currentTurretPower; @@ -55,7 +48,27 @@ public class Robot extends TimedRobot { 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 pitchChange = 0; + double defCollectorPower = 1; + double defIndexerPower = 1; + double curCollector; + double curIndexer; + + double newPos; PIDController turretPIDController; @@ -79,19 +92,13 @@ public class Robot extends TimedRobot { maxCurrentRightB = robot.getRightBCurrent(); } - /* 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"); - -//read values periodically - turretx = tx.getDouble(0.0); - turrety = ty.getDouble(0.0); - turretarea = ta.getDouble(0.0); - turretv = tv.getDouble(0.0); -*/ - SmartDashboard.putBoolean("tv", robot.isTargetFound()); + + 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("LeftACurrentMax", maxCurrentLeftA); SmartDashboard.putNumber("LeftBCurrentMax", maxCurrentLeftB); SmartDashboard.putNumber("RightACurrentMax", maxCurrentRightA); @@ -133,6 +140,7 @@ 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 y", robot.getTargetY()); SmartDashboard.putNumber("Vision target angle", robot.getTargetAngle()); @@ -221,34 +229,24 @@ public class Robot extends TimedRobot { reset = true; //////////////////////////////////////////////////DRIVETRAIN CONTROL - 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; - //moved this to after joystick deaden because deaden should be focused on the raw joystick values - double scaleFactor = 1.0; - if (!robot.getPTOState()) { - robot.drivePercent( - (jy + jx) * scaleFactor, - (jy - jx) * scaleFactor - ); + + joystickX = joystick.getX(); + joystickY = -joystick.getY(); + + if (joystickY > -cutoff && joystickY < cutoff) { + joystickY = 0; + } + if (joystickX > -cutoff && joystickX < cutoff){ + joystickX = 0; } - //////////////////////////////////////////////DRIVETRAIN CONTROL ENDS - ///////////////////////////////////////////////////////////////////////SET UP XBOX - 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)); + if (!robot.getPTOState()) { + driveLeft = (joystickY + joystickX) * scaleFactor; + driveRight = (joystickY - joystickX) * scaleFactor; + } + //////////////////////////////////////////////DRIVETRAIN CONTROL ENDS - int leftAxis = 1; - int rightAxis = 5; - double tolerance = 0.8; - double drivePower = 0.2; - ///////////////////////////////////////////////////////////////////SET UP OVER //////////////////////////////////////////////////////////TURRET CONTROL if (joystick.getRawButton(12)) { @@ -265,35 +263,33 @@ else if (joystick.getRawButton(15)) { /////////////////////////////////////////////////////CLIMBER WHEN PTO - double driveLeft = 0; - double driveRight = 0; + if (robot.getPTOState()) { if (xbox.getRawAxis(leftAxis) > tolerance) { - driveLeft = drivePower; + driveLeft = defaultClimbPower; } else if (xbox.getRawAxis(leftAxis) < -tolerance) { - driveLeft = -drivePower; + driveLeft = -defaultClimbPower; } if (xbox.getRawAxis(rightAxis) > tolerance) { - driveRight = drivePower; + driveRight = defaultClimbPower; } else if (xbox.getRawAxis(rightAxis) < -tolerance) { - driveRight = -drivePower; + driveRight = -defaultClimbPower; } - robot.drivePercent(driveLeft, driveRight); } /////////////////////////////////////////////////////CLIMBER CODE ENDS //////////////////////////////////////////////////////////SHOOTER CODE BEGINS - double shooterTargetRPM = 0; + if (joystick.getRawButton(11)) { shooterTargetRPM = robot.getShooterTargetRPM(); } else { shooterTargetRPM = 0; } - robot.setShooterRPM(shooterTargetRPM, shooterTargetRPM); + if (joystick.getRawButton(16)) { if (robot.isReadyToShoot()) { @@ -313,7 +309,7 @@ else if (joystick.getRawButton(15)) { ///////////////////////////////////////////////////////////ACTUATOR STUFF - double pitchChange = 0; + if (joystick.getRawButtonPressed(13)){ pitchChange = 0.02; } @@ -323,17 +319,14 @@ else if (joystick.getRawButtonPressed(14)){ else{ pitchChange = 0; } - double newPos = robot.getTurretPitchPosition() + pitchChange; + newPos = robot.getTurretPitchPosition() + pitchChange; + - robot.setTurretPitchPosition(newPos); /////////////////////////////////////////////////////////////////ACTUATOR STUFF ENDS /////////////////////////////////////////////////////////COLLECTOR CONTROLS - double defCollectorPower = 1; - double defIndexerPower = 1; - double curCollector; - double curIndexer; + //button 2 = bottom center button if(joystick.getRawButton(2)){ @@ -368,10 +361,19 @@ else if (joystick.getRawButton(5)){ curIndexer = defIndexerPower; } + //////////////////////////////////////////////////COLLECTOR LOGIC ENDS + + + + ///////////////////////////////////////////////////POWER SETTERS + + robot.drivePercent(driveLeft, driveRight); + robot.setShooterRPM(shooterTargetRPM, shooterTargetRPM); + robot.setTurretPitchPosition(newPos); robot.setCollectorIntakePercentage(curCollector); robot.setIndexerIntakePercentage(curIndexer); - //////////////////////////////////////////////////COLLECTOR LOGIC ENDS + //////////////////////////////////////////////////POWER SETTERS END } if (hang) { From e60254e7a5de75e7b5e5455730ec40fca77a7557 Mon Sep 17 00:00:00 2001 From: Malti Date: Fri, 25 Feb 2022 20:25:18 -0500 Subject: [PATCH 33/72] shooter code with buttons --- src/main/java/frc/robot/Robot.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a7a98e0..02eed1d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -300,7 +300,7 @@ public class Robot extends TimedRobot { //////////////////////////////////////////////////////////SHOOTER CODE BEGINS - if (joystick.getRawButton(11)) { + if (xbox.getRawButton(3)) { shooterTargetRPM = robot.getShooterTargetRPM(); } else { shooterTargetRPM = 0; @@ -308,7 +308,7 @@ public class Robot extends TimedRobot { - if (joystick.getRawButton(16)) { + if (xbox.getRawButton(1)) { if (robot.isReadyToShoot()) { robot.setActivelyShooting(true); } else { From c529ad15b779b9d458364e77ab835213b878d739 Mon Sep 17 00:00:00 2001 From: Louminator Date: Fri, 25 Feb 2022 20:25:48 -0500 Subject: [PATCH 34/72] collector buttons --- src/main/java/frc/robot/Robot.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a7a98e0..f4e1a09 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -346,7 +346,7 @@ else if (joystick.getRawButtonPressed(14)){ //button 2 = bottom center button - if(joystick.getRawButton(2)){ + if(xbox.getRawButton(4)){ if(!robot.getUpperCargo()){ curCollector = defCollectorPower; curIndexer = defIndexerPower; @@ -364,7 +364,7 @@ else if (joystick.getRawButtonPressed(14)){ curIndexer = defIndexerPower; } } - else if (joystick.getRawButton(5)){ + else if (xbox.getRawButton(2)){ curCollector = -defCollectorPower; curIndexer = -defIndexerPower; } From c3959d262e1a22d22d089eb076f9aab3fd80136c Mon Sep 17 00:00:00 2001 From: Louminator Date: Fri, 25 Feb 2022 20:34:09 -0500 Subject: [PATCH 35/72] Shooting setpoints --- src/main/java/frc/robot/Robot.java | 49 ++++++++++++++++++++++++++++++ 1 file changed, 49 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9cd5642..d6a4698 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -14,6 +14,11 @@ import frc.robot.generic.GenericRobot; import frc.robot.generic.Lightning; +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 @@ -215,6 +220,15 @@ public class Robot extends TimedRobot { @Override public void teleopPeriodic() { double turretPower = 0; + double targetRPM=0; + double turretPitch=0; + + switch (POVDirection.getDirection(xbox.getPOV())) { + case NORTH: targetRPM = 3250; turretPitch = 0.00; break; + case EAST : targetRPM = 4000; turretPitch = 0.00; break; + case SOUTH: targetRPM = 5000; turretPitch = 0.00; break; + case WEST : targetRPM = 5500; turretPitch = 0.00; break; + } //note to self: buttons currently assume mirrored joystick setting if (joystick.getRawButtonPressed(8)) { @@ -534,5 +548,40 @@ else if (joystick.getRawButton(14)){ 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 From b567d88bf19f19eb96def2d5554f829900ee8756 Mon Sep 17 00:00:00 2001 From: Louminator Date: Fri, 25 Feb 2022 20:36:41 -0500 Subject: [PATCH 36/72] Shooting setpoints --- src/main/java/frc/robot/Robot.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d6a4698..d69837b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -224,7 +224,7 @@ public class Robot extends TimedRobot { double turretPitch=0; switch (POVDirection.getDirection(xbox.getPOV())) { - case NORTH: targetRPM = 3250; turretPitch = 0.00; break; + case NORTH: targetRPM = 5500; turretPitch = 0.38; break; case EAST : targetRPM = 4000; turretPitch = 0.00; break; case SOUTH: targetRPM = 5000; turretPitch = 0.00; break; case WEST : targetRPM = 5500; turretPitch = 0.00; break; @@ -400,7 +400,7 @@ else if (xbox.getRawButton(2)){ robot.drivePercent(driveLeft, driveRight); robot.setShooterRPM(shooterTargetRPM, shooterTargetRPM); - robot.setTurretPitchPosition(newPos); + robot.setTurretPitchPosition(turretPitch); robot.setTurretPowerPct(turretPower); robot.setCollectorIntakePercentage(curCollector); robot.setIndexerIntakePercentage(curIndexer); From f92b8991f6b94c16d2a5d5cdb97bb9f98e8bf8cb Mon Sep 17 00:00:00 2001 From: Malti Date: Fri, 25 Feb 2022 20:51:53 -0500 Subject: [PATCH 37/72] Actuator with presets working. --- src/main/java/frc/robot/Robot.java | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d69837b..41728a1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -315,7 +315,7 @@ public class Robot extends TimedRobot { //////////////////////////////////////////////////////////SHOOTER CODE BEGINS if (xbox.getRawButton(3)) { - shooterTargetRPM = robot.getShooterTargetRPM(); + shooterTargetRPM = targetRPM; } else { shooterTargetRPM = 0; } @@ -342,15 +342,12 @@ public class Robot extends TimedRobot { ///////////////////////////////////////////////////////////ACTUATOR STUFF if (joystick.getRawButtonPressed(13)){ - pitchChange = 0.02; + turretPitch = robot.getTurretPitchPosition() + .02; } else if (joystick.getRawButtonPressed(14)){ pitchChange = -0.02; + turretPitch = robot.getTurretPitchPosition() + .02; } - else{ - pitchChange = 0; - } - newPos = robot.getTurretPitchPosition() + pitchChange; /////////////////////////////////////////////////////////////////ACTUATOR STUFF ENDS From 4e86f10a89ffeff290d850698fb545a6793b10bf Mon Sep 17 00:00:00 2001 From: Malti Date: Fri, 25 Feb 2022 20:53:17 -0500 Subject: [PATCH 38/72] Actuator presets --- src/main/java/frc/robot/Robot.java | 256 +++++++++++++++-------------- 1 file changed, 132 insertions(+), 124 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 41728a1..ba34282 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -24,11 +24,11 @@ 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 BallAtoTerminal(), - simpleBTerminal = new BallBtoTerminal(), - simpleCTerminal = new BallCtoTerminal(), - CTerminalReturn = new BallCtoTerminalReturn(); + autoArc = new autoArc(), + simpleATerminal = new BallAtoTerminal(), + simpleBTerminal = new BallBtoTerminal(), + simpleCTerminal = new BallCtoTerminal(), + CTerminalReturn = new BallCtoTerminalReturn(); GenericRobot robot = new Lightning(); Joystick joystick = new Joystick(0); @@ -38,7 +38,7 @@ public class Robot extends TimedRobot { int averageTurretXSize = 2; - double[] averageX = new double [averageTurretXSize]; + double[] averageX = new double[averageTurretXSize]; int counter = 0; @@ -79,21 +79,23 @@ public class Robot extends TimedRobot { PIDController turretPIDController; + @Override + public void robotInit() { + } - @Override public void robotInit() {} - - @Override public void robotPeriodic() { + @Override + public void robotPeriodic() { - if (robot.getLeftACurrent() > maxCurrentLeftA){ + if (robot.getLeftACurrent() > maxCurrentLeftA) { maxCurrentLeftA = robot.getLeftACurrent(); } - if (robot.getLeftBCurrent() > maxCurrentLeftB){ + if (robot.getLeftBCurrent() > maxCurrentLeftB) { maxCurrentLeftB = robot.getLeftBCurrent(); } - if(robot.getRightACurrent() > maxCurrentRightA){ + if (robot.getRightACurrent() > maxCurrentRightA) { maxCurrentRightA = robot.getRightACurrent(); } - if (robot.getRightBCurrent() > maxCurrentRightB){ + if (robot.getRightBCurrent() > maxCurrentRightB) { maxCurrentRightB = robot.getRightBCurrent(); } @@ -203,31 +205,47 @@ public class Robot extends TimedRobot { } - @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()); - hang = false; - count = 0; + @Override + public void teleopInit() { + turretPIDController = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD()); + hang = false; + count = 0; } - @Override public void teleopPeriodic() { + @Override + public void teleopPeriodic() { double turretPower = 0; - double targetRPM=0; - double turretPitch=0; + double targetRPM = 0; + double turretPitch = 0; switch (POVDirection.getDirection(xbox.getPOV())) { - case NORTH: targetRPM = 5500; turretPitch = 0.38; break; - case EAST : targetRPM = 4000; turretPitch = 0.00; break; - case SOUTH: targetRPM = 5000; turretPitch = 0.00; break; - case WEST : targetRPM = 5500; turretPitch = 0.00; break; + case NORTH: + targetRPM = 5500; + turretPitch = 0.38; + break; + case EAST: + targetRPM = 4000; + turretPitch = 0.00; + break; + case SOUTH: + targetRPM = 5000; + turretPitch = 0.00; + break; + case WEST: + targetRPM = 5500; + turretPitch = 0.00; + break; } //note to self: buttons currently assume mirrored joystick setting @@ -252,7 +270,7 @@ public class Robot extends TimedRobot { if (joystickY > -cutoff && joystickY < cutoff) { joystickY = 0; } - if (joystickX > -cutoff && joystickX < cutoff){ + if (joystickX > -cutoff && joystickX < cutoff) { joystickX = 0; } @@ -272,15 +290,12 @@ public class Robot extends TimedRobot { double average = 0; for (int i = 0; i < averageTurretXSize; i++) - average += averageX[i]; + average += averageX[i]; average /= averageTurretXSize; - if ((xbox.getRawAxis(4) > 0.80) & robot.isTargetFound()) - { + if ((xbox.getRawAxis(4) > 0.80) & robot.isTargetFound()) { turretPower = turretPIDController.calculate(average); - } - else - { + } else { turretPIDController.reset(); if (xbox.getRawButton(5)) { turretPower = -0.45; @@ -297,22 +312,21 @@ public class Robot extends TimedRobot { if (robot.getPTOState()) { if (xbox.getRawAxis(leftAxis) > tolerance) { - driveLeft = defaultClimbPower; + driveLeft = defaultClimbPower; } else if (xbox.getRawAxis(leftAxis) < -tolerance) { - driveLeft = -defaultClimbPower; + driveLeft = -defaultClimbPower; } if (xbox.getRawAxis(rightAxis) > tolerance) { - driveRight = defaultClimbPower; + driveRight = defaultClimbPower; } else if (xbox.getRawAxis(rightAxis) < -tolerance) { - driveRight = -defaultClimbPower; + driveRight = -defaultClimbPower; } } - /////////////////////////////////////////////////////CLIMBER CODE ENDS - + /////////////////////////////////////////////////////CLIMBER CODE ENDS - //////////////////////////////////////////////////////////SHOOTER CODE BEGINS + //////////////////////////////////////////////////////////SHOOTER CODE BEGINS if (xbox.getRawButton(3)) { shooterTargetRPM = targetRPM; @@ -321,7 +335,6 @@ public class Robot extends TimedRobot { } - if (xbox.getRawButton(1)) { if (robot.isReadyToShoot()) { robot.setActivelyShooting(true); @@ -336,50 +349,46 @@ public class Robot extends TimedRobot { if (joystick.getRawButton(7)) { robot.setActivelyShooting(true); } - ////////////////////////////////////////////////////////////SHOOTER CODE ENDS + ////////////////////////////////////////////////////////////SHOOTER CODE ENDS - ///////////////////////////////////////////////////////////ACTUATOR STUFF + ///////////////////////////////////////////////////////////ACTUATOR STUFF - if (joystick.getRawButtonPressed(13)){ + if (joystick.getRawButtonPressed(13)) { turretPitch = robot.getTurretPitchPosition() + .02; } - else if (joystick.getRawButtonPressed(14)){ + else if (joystick.getRawButtonPressed(14)) { pitchChange = -0.02; turretPitch = robot.getTurretPitchPosition() + .02; } - /////////////////////////////////////////////////////////////////ACTUATOR STUFF ENDS + /////////////////////////////////////////////////////////////////ACTUATOR STUFF ENDS - /////////////////////////////////////////////////////////COLLECTOR CONTROLS + /////////////////////////////////////////////////////////COLLECTOR CONTROLS //button 2 = bottom center button - if(xbox.getRawButton(4)){ - if(!robot.getUpperCargo()){ + if (xbox.getRawButton(4)) { + if (!robot.getUpperCargo()) { curCollector = defCollectorPower; curIndexer = defIndexerPower; - } - else{ + } else { curIndexer = 0; - if(!robot.getLowerCargo()){ + if (!robot.getLowerCargo()) { curCollector = defCollectorPower; - } - else{ + } else { curCollector = 0; } } - if(robot.isActivelyShooting()){ + if (robot.isActivelyShooting()) { curIndexer = defIndexerPower; } - } - else if (xbox.getRawButton(2)){ + } else if (xbox.getRawButton(2)) { curCollector = -defCollectorPower; curIndexer = -defIndexerPower; - } - else{ + } else { curCollector = 0; curIndexer = 0; } @@ -389,11 +398,10 @@ else if (xbox.getRawButton(2)){ curIndexer = defIndexerPower; } - //////////////////////////////////////////////////COLLECTOR LOGIC ENDS - + //////////////////////////////////////////////////COLLECTOR LOGIC ENDS - ///////////////////////////////////////////////////POWER SETTERS + ///////////////////////////////////////////////////POWER SETTERS robot.drivePercent(driveLeft, driveRight); robot.setShooterRPM(shooterTargetRPM, shooterTargetRPM); @@ -402,7 +410,7 @@ else if (xbox.getRawButton(2)){ robot.setCollectorIntakePercentage(curCollector); robot.setIndexerIntakePercentage(curIndexer); - //////////////////////////////////////////////////POWER SETTERS END + //////////////////////////////////////////////////POWER SETTERS END } if (hang) { @@ -419,12 +427,15 @@ else if (xbox.getRawButton(2)){ } - @Override public void disabledInit() {} + @Override + public void disabledInit() { + } - @Override public void disabledPeriodic() { + @Override + public void disabledPeriodic() { hang = false; count = 0; - if (joystick.getRawButton(1)){ + if (joystick.getRawButton(1)) { robot.resetAttitude(); robot.resetEncoders(); } @@ -437,134 +448,130 @@ else if (xbox.getRawButton(2)){ } - @Override public void testInit() {} + @Override + public void testInit() { + } - @Override public void testPeriodic() { + @Override + public void testPeriodic() { - double pitchChange = 0; - if (xbox.getRawButton(1)){ + double pitchChange = 0; + if (xbox.getRawButton(1)) { pitchChange = 0.02; - } - else if (xbox.getRawButton(2)){ + } else if (xbox.getRawButton(2)) { pitchChange = -0.02; - } - else{ + } else { pitchChange = 0; } - double newPos = robot.getTurretPitchPosition() + pitchChange; - if(newPos < 0) newPos = 0; - if(newPos > 1) newPos = 1; + double newPos = robot.getTurretPitchPosition() + pitchChange; + if (newPos < 0) newPos = 0; + if (newPos > 1) newPos = 1; - robot.setTurretPitchPosition(newPos); + 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; - if(!robot.getPTOState()){ + if (!robot.getPTOState()) { robot.drivePercent( - (driveY+driveX) * scaleFactor, - (driveY-driveX) * scaleFactor + (driveY + driveX) * scaleFactor, + (driveY - driveX) * scaleFactor ); } - int leftAxis = 1; int rightAxis = 5; + int leftAxis = 1; + int rightAxis = 5; double tolerance = 0.8; double drivePower = 0.2; - 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); double driveLeft = 0; double driveRight = 0; - if(robot.getPTOState()){ - if(xbox.getRawAxis(leftAxis) > tolerance){ + if (robot.getPTOState()) { + if (xbox.getRawAxis(leftAxis) > tolerance) { driveLeft = drivePower; - } - else if(xbox.getRawAxis(leftAxis) < -tolerance){ + } else if (xbox.getRawAxis(leftAxis) < -tolerance) { driveLeft = -drivePower; } - if(xbox.getRawAxis(rightAxis) > tolerance){ + if (xbox.getRawAxis(rightAxis) > tolerance) { driveRight = drivePower; - } - else if(xbox.getRawAxis(rightAxis) < -tolerance){ + } 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( 6)) robot.turnOnPTO(); - if (joystick.getRawButton( 9)) robot.turnOffPTO(); + if (joystick.getRawButton(7)) robot.raiseCollector(); + if (joystick.getRawButton(8)) robot.lowerCollector(); - if (joystick.getRawButton( 5)) robot.setArmsForward(); - if (joystick.getRawButton(10)) robot.setArmsBackward(); + 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); + 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; } @@ -581,4 +588,5 @@ public int getAngle() { public static POVDirection getDirection(int angle) { return directionMap.getOrDefault(angle, POVDirection.NULL); } + } } \ No newline at end of file From 6a6a4dbfef45edec40200b9dde581e3eb6a8793b Mon Sep 17 00:00:00 2001 From: Louminator Date: Fri, 25 Feb 2022 20:53:19 -0500 Subject: [PATCH 39/72] Shooting setpoints --- src/main/java/frc/robot/Robot.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d69837b..0a6ce31 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -341,6 +341,10 @@ public class Robot extends TimedRobot { ///////////////////////////////////////////////////////////ACTUATOR STUFF + double deadzone = 0.1; + double rJoyRY = xbox.getRawAxis( 5); + if (rJoyRY > -deadzone && rJoyRY < deadzone) rJoyRY = 0; + turretPitch += rJoyRY * 0.05; if (joystick.getRawButtonPressed(13)){ pitchChange = 0.02; } From 2dfe0203c81739da42ebb255a5d72181a23b4edf Mon Sep 17 00:00:00 2001 From: Louminator Date: Fri, 25 Feb 2022 21:03:01 -0500 Subject: [PATCH 40/72] Manual actuator control --- src/main/java/frc/robot/Robot.java | 246 ++++++++++++++--------------- 1 file changed, 118 insertions(+), 128 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d97be3d..cc43c75 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -24,11 +24,11 @@ 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 BallAtoTerminal(), - simpleBTerminal = new BallBtoTerminal(), - simpleCTerminal = new BallCtoTerminal(), - CTerminalReturn = new BallCtoTerminalReturn(); + autoArc = new autoArc(), + simpleATerminal = new BallAtoTerminal(), + simpleBTerminal = new BallBtoTerminal(), + simpleCTerminal = new BallCtoTerminal(), + CTerminalReturn = new BallCtoTerminalReturn(); GenericRobot robot = new Lightning(); Joystick joystick = new Joystick(0); @@ -38,7 +38,7 @@ public class Robot extends TimedRobot { int averageTurretXSize = 2; - double[] averageX = new double [averageTurretXSize]; + double[] averageX = new double[averageTurretXSize]; int counter = 0; @@ -79,21 +79,23 @@ public class Robot extends TimedRobot { PIDController turretPIDController; + @Override + public void robotInit() { + } - @Override public void robotInit() {} - - @Override public void robotPeriodic() { + @Override + public void robotPeriodic() { - if (robot.getLeftACurrent() > maxCurrentLeftA){ + if (robot.getLeftACurrent() > maxCurrentLeftA) { maxCurrentLeftA = robot.getLeftACurrent(); } - if (robot.getLeftBCurrent() > maxCurrentLeftB){ + if (robot.getLeftBCurrent() > maxCurrentLeftB) { maxCurrentLeftB = robot.getLeftBCurrent(); } - if(robot.getRightACurrent() > maxCurrentRightA){ + if (robot.getRightACurrent() > maxCurrentRightA) { maxCurrentRightA = robot.getRightACurrent(); } - if (robot.getRightBCurrent() > maxCurrentRightB){ + if (robot.getRightBCurrent() > maxCurrentRightB) { maxCurrentRightB = robot.getRightBCurrent(); } @@ -203,25 +205,29 @@ public class Robot extends TimedRobot { } - @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()); - hang = false; - count = 0; + @Override + public void teleopInit() { + turretPIDController = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD()); + hang = false; + count = 0; } - @Override public void teleopPeriodic() { + @Override + public void teleopPeriodic() { double turretPower = 0; - double targetRPM=0; - double turretPitch=0; + double targetRPM = 0; + double turretPitch = 0; switch (POVDirection.getDirection(xbox.getPOV())) { case NORTH: @@ -264,7 +270,7 @@ public class Robot extends TimedRobot { if (joystickY > -cutoff && joystickY < cutoff) { joystickY = 0; } - if (joystickX > -cutoff && joystickX < cutoff){ + if (joystickX > -cutoff && joystickX < cutoff) { joystickX = 0; } @@ -284,15 +290,12 @@ public class Robot extends TimedRobot { double average = 0; for (int i = 0; i < averageTurretXSize; i++) - average += averageX[i]; + average += averageX[i]; average /= averageTurretXSize; - if ((xbox.getRawAxis(4) > 0.80) & robot.isTargetFound()) - { + if ((xbox.getRawAxis(4) > 0.80) & robot.isTargetFound()) { turretPower = turretPIDController.calculate(average); - } - else - { + } else { turretPIDController.reset(); if (xbox.getRawButton(5)) { turretPower = -0.45; @@ -309,22 +312,21 @@ public class Robot extends TimedRobot { if (robot.getPTOState()) { if (xbox.getRawAxis(leftAxis) > tolerance) { - driveLeft = defaultClimbPower; + driveLeft = defaultClimbPower; } else if (xbox.getRawAxis(leftAxis) < -tolerance) { - driveLeft = -defaultClimbPower; + driveLeft = -defaultClimbPower; } if (xbox.getRawAxis(rightAxis) > tolerance) { - driveRight = defaultClimbPower; + driveRight = defaultClimbPower; } else if (xbox.getRawAxis(rightAxis) < -tolerance) { - driveRight = -defaultClimbPower; + driveRight = -defaultClimbPower; } } - /////////////////////////////////////////////////////CLIMBER CODE ENDS + /////////////////////////////////////////////////////CLIMBER CODE ENDS - - //////////////////////////////////////////////////////////SHOOTER CODE BEGINS + //////////////////////////////////////////////////////////SHOOTER CODE BEGINS if (xbox.getRawButton(3)) { shooterTargetRPM = targetRPM; @@ -333,7 +335,6 @@ public class Robot extends TimedRobot { } - if (xbox.getRawButton(1)) { if (robot.isReadyToShoot()) { robot.setActivelyShooting(true); @@ -348,28 +349,22 @@ public class Robot extends TimedRobot { if (joystick.getRawButton(7)) { robot.setActivelyShooting(true); } - ////////////////////////////////////////////////////////////SHOOTER CODE ENDS + ////////////////////////////////////////////////////////////SHOOTER CODE ENDS - ///////////////////////////////////////////////////////////ACTUATOR STUFF + ///////////////////////////////////////////////////////////ACTUATOR STUFF double deadzone = 0.1; - double rJoyRY = xbox.getRawAxis( 5); + double rJoyRY = xbox.getRawAxis(5); if (rJoyRY > -deadzone && rJoyRY < deadzone) rJoyRY = 0; - turretPitch += rJoyRY * 0.05; - if (joystick.getRawButtonPressed(13)){ - pitchChange = 0.02; - if (joystick.getRawButtonPressed(13)) { - turretPitch = robot.getTurretPitchPosition() + .02; - } - else if (joystick.getRawButtonPressed(14)) { - pitchChange = -0.02; + + if (rJoyRY > deadzone) { turretPitch = robot.getTurretPitchPosition() + .02; } - else{ - pitchChange = 0; + + if (rJoyRY < -deadzone) { + turretPitch = robot.getTurretPitchPosition() - .02; } - newPos = robot.getTurretPitchPosition() + pitchChange; /////////////////////////////////////////////////////////////////ACTUATOR STUFF ENDS @@ -379,29 +374,25 @@ else if (joystick.getRawButtonPressed(14)) { //button 2 = bottom center button - if(xbox.getRawButton(4)){ - if(!robot.getUpperCargo()){ + if (xbox.getRawButton(4)) { + if (!robot.getUpperCargo()) { curCollector = defCollectorPower; curIndexer = defIndexerPower; - } - else{ + } else { curIndexer = 0; - if(!robot.getLowerCargo()){ + if (!robot.getLowerCargo()) { curCollector = defCollectorPower; - } - else{ + } else { curCollector = 0; } } - if(robot.isActivelyShooting()){ + if (robot.isActivelyShooting()) { curIndexer = defIndexerPower; } - } - else if (xbox.getRawButton(2)){ + } else if (xbox.getRawButton(2)) { curCollector = -defCollectorPower; curIndexer = -defIndexerPower; - } - else{ + } else { curCollector = 0; curIndexer = 0; } @@ -411,11 +402,10 @@ else if (xbox.getRawButton(2)){ curIndexer = defIndexerPower; } - //////////////////////////////////////////////////COLLECTOR LOGIC ENDS - + //////////////////////////////////////////////////COLLECTOR LOGIC ENDS - ///////////////////////////////////////////////////POWER SETTERS + ///////////////////////////////////////////////////POWER SETTERS robot.drivePercent(driveLeft, driveRight); robot.setShooterRPM(shooterTargetRPM, shooterTargetRPM); @@ -424,7 +414,7 @@ else if (xbox.getRawButton(2)){ robot.setCollectorIntakePercentage(curCollector); robot.setIndexerIntakePercentage(curIndexer); - //////////////////////////////////////////////////POWER SETTERS END + //////////////////////////////////////////////////POWER SETTERS END } if (hang) { @@ -441,12 +431,15 @@ else if (xbox.getRawButton(2)){ } - @Override public void disabledInit() {} + @Override + public void disabledInit() { + } - @Override public void disabledPeriodic() { + @Override + public void disabledPeriodic() { hang = false; count = 0; - if (joystick.getRawButton(1)){ + if (joystick.getRawButton(1)) { robot.resetAttitude(); robot.resetEncoders(); } @@ -459,134 +452,130 @@ else if (xbox.getRawButton(2)){ } - @Override public void testInit() {} + @Override + public void testInit() { + } - @Override public void testPeriodic() { + @Override + public void testPeriodic() { - double pitchChange = 0; - if (xbox.getRawButton(1)){ + double pitchChange = 0; + if (xbox.getRawButton(1)) { pitchChange = 0.02; - } - else if (xbox.getRawButton(2)){ + } else if (xbox.getRawButton(2)) { pitchChange = -0.02; - } - else{ + } else { pitchChange = 0; } - double newPos = robot.getTurretPitchPosition() + pitchChange; - if(newPos < 0) newPos = 0; - if(newPos > 1) newPos = 1; + double newPos = robot.getTurretPitchPosition() + pitchChange; + if (newPos < 0) newPos = 0; + if (newPos > 1) newPos = 1; - robot.setTurretPitchPosition(newPos); + 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; - if(!robot.getPTOState()){ + if (!robot.getPTOState()) { robot.drivePercent( - (driveY+driveX) * scaleFactor, - (driveY-driveX) * scaleFactor + (driveY + driveX) * scaleFactor, + (driveY - driveX) * scaleFactor ); } - int leftAxis = 1; int rightAxis = 5; + int leftAxis = 1; + int rightAxis = 5; double tolerance = 0.8; double drivePower = 0.2; - 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); double driveLeft = 0; double driveRight = 0; - if(robot.getPTOState()){ - if(xbox.getRawAxis(leftAxis) > tolerance){ + if (robot.getPTOState()) { + if (xbox.getRawAxis(leftAxis) > tolerance) { driveLeft = drivePower; - } - else if(xbox.getRawAxis(leftAxis) < -tolerance){ + } else if (xbox.getRawAxis(leftAxis) < -tolerance) { driveLeft = -drivePower; } - if(xbox.getRawAxis(rightAxis) > tolerance){ + if (xbox.getRawAxis(rightAxis) > tolerance) { driveRight = drivePower; - } - else if(xbox.getRawAxis(rightAxis) < -tolerance){ + } 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( 6)) robot.turnOnPTO(); - if (joystick.getRawButton( 9)) robot.turnOffPTO(); + if (joystick.getRawButton(7)) robot.raiseCollector(); + if (joystick.getRawButton(8)) robot.lowerCollector(); - if (joystick.getRawButton( 5)) robot.setArmsForward(); - if (joystick.getRawButton(10)) robot.setArmsBackward(); + 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); + 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; } @@ -603,4 +592,5 @@ public int getAngle() { public static POVDirection getDirection(int angle) { return directionMap.getOrDefault(angle, POVDirection.NULL); } + } } \ No newline at end of file From d47b8df45cfc028d5d66dfa951dffb018329210d Mon Sep 17 00:00:00 2001 From: Louminator Date: Sat, 26 Feb 2022 09:34:22 -0500 Subject: [PATCH 41/72] Reverse turret sense. Put in turret stops. --- src/main/java/frc/robot/generic/Lightning.java | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/generic/Lightning.java b/src/main/java/frc/robot/generic/Lightning.java index 7a753ba..5885566 100644 --- a/src/main/java/frc/robot/generic/Lightning.java +++ b/src/main/java/frc/robot/generic/Lightning.java @@ -332,7 +332,15 @@ public void setTurretAngleAbsolute() { @Override public void setTurretPowerPct(double powerPct) { - turretRotator.set(powerPct); + if ( (getAlternateTurretAngleDegrees()>355) & (powerPct>0)) + { + powerPct = 0; + } + if ( (getAlternateTurretAngleDegrees()>5) & (powerPct<0)) + { + powerPct = 0; + } + turretRotator.set(-powerPct); } @Override From 3d62567e47576e1c5ae66f5aaeaf50b3f48e54f7 Mon Sep 17 00:00:00 2001 From: Malti Date: Sat, 26 Feb 2022 12:19:35 -0500 Subject: [PATCH 42/72] Changes from testing in the lab today. --- src/main/java/frc/robot/Robot.java | 28 +++++----- .../autonomous/BallCtoTerminalReturn.java | 51 ++++++++----------- .../java/frc/robot/generic/Lightning.java | 26 ++++++---- 3 files changed, 53 insertions(+), 52 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cc43c75..119bee5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -28,7 +28,8 @@ public class Robot extends TimedRobot { simpleATerminal = new BallAtoTerminal(), simpleBTerminal = new BallBtoTerminal(), simpleCTerminal = new BallCtoTerminal(), - CTerminalReturn = new BallCtoTerminalReturn(); + CTerminalReturn = new BallCtoTerminalReturn(), + simpleB = new BallSimpleB(); GenericRobot robot = new Lightning(); Joystick joystick = new Joystick(0); @@ -73,7 +74,8 @@ public class Robot extends TimedRobot { double curCollector; double curIndexer; - double newPos; + double targetRPM = 0; + double turretPitch = 0; PIDController turretPIDController; @@ -149,13 +151,14 @@ public void robotPeriodic() { 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("Alternate turret angle degrees", robot.getAlternateTurretAngle()); SmartDashboard.putNumber("Turret direction motor pct", robot.getTurretPowerPct()); @@ -224,10 +227,8 @@ public void teleopInit() { @Override public void teleopPeriodic() { + turretPower = 0; - double turretPower = 0; - double targetRPM = 0; - double turretPitch = 0; switch (POVDirection.getDirection(xbox.getPOV())) { case NORTH: @@ -286,20 +287,19 @@ public void teleopPeriodic() { counter = (counter + 1) % averageTurretXSize; averageX[counter] = robot.getTargetX(); - turretPIDController.calculate(average); - double average = 0; + average = 0; for (int i = 0; i < averageTurretXSize; i++) average += averageX[i]; average /= averageTurretXSize; - if ((xbox.getRawAxis(4) > 0.80) & robot.isTargetFound()) { + if ((xbox.getRawAxis(2) > 0.10) & robot.isTargetFound()) { turretPower = turretPIDController.calculate(average); } else { turretPIDController.reset(); - if (xbox.getRawButton(5)) { + if (xbox.getRawButton(6)) { turretPower = -0.45; - } else if (xbox.getRawButton(6)) { + } else if (xbox.getRawButton(5)) { turretPower = 0.45; } else { turretPower = 0.0; @@ -374,7 +374,7 @@ public void teleopPeriodic() { //button 2 = bottom center button - if (xbox.getRawButton(4)) { + if (xbox.getRawButton(2)) { if (!robot.getUpperCargo()) { curCollector = defCollectorPower; curIndexer = defIndexerPower; @@ -389,7 +389,7 @@ public void teleopPeriodic() { if (robot.isActivelyShooting()) { curIndexer = defIndexerPower; } - } else if (xbox.getRawButton(2)) { + } else if (xbox.getRawButton(4)) { curCollector = -defCollectorPower; curIndexer = -defIndexerPower; } else { @@ -437,6 +437,7 @@ public void disabledInit() { @Override public void disabledPeriodic() { + joystick.getRawButtonPressed(8); hang = false; count = 0; if (joystick.getRawButton(1)) { @@ -449,6 +450,7 @@ public void disabledPeriodic() { if (joystick.getRawButton(6)) autonomous = simpleBTerminal; if (joystick.getRawButton(7)) autonomous = simpleCTerminal; if (joystick.getRawButton(9)) autonomous = CTerminalReturn; + if (joystick.getRawButton(11)) autonomous = simpleB; } diff --git a/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java index 23f227d..9579bf8 100644 --- a/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java +++ b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java @@ -81,8 +81,11 @@ public void autonomousPeriodic(GenericRobot robot) { - robot.getCargo(); - robot.shoot(); + if (autonomousStep >= 1){ + robot.getCargo(); + robot.shoot(); + robot.setTurretPitchPosition(.38); + } switch(autonomousStep){ case 0: //reset robot.lowerCollector(); @@ -98,20 +101,8 @@ public void autonomousPeriodic(GenericRobot robot) { 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: //turn the shooter off - if (System.currentTimeMillis() - startTime >= 250){ - robot.setActivelyShooting(false); - autonomousStep += 1; - } - break; - case 3: //drive to ball C + + case 1: //drive to ball C collectorPct = 1; correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); @@ -129,34 +120,34 @@ public void autonomousPeriodic(GenericRobot robot) { startTime = System.currentTimeMillis(); } break; - case 4: //stop + case 2: //stop leftpower = 0; rightpower = 0; startDistance = robot.getDriveDistanceInchesLeft(); - autonomousStep = 12; + autonomousStep += 1; time = false; break; - case 5: //collect cargo + case 3: //create a target shooter value and see if shooter reaches it. if (robot.canShoot()){ robot.setActivelyShooting(true); startTime = System.currentTimeMillis(); - autonomousStep += 1.0; + autonomousStep += 1; } break; - case 6: //sets shooter's RPM - if (System.currentTimeMillis() - startTime >= 500){ + case 4: //turn the shooter off + if (System.currentTimeMillis() - startTime >= 1000){ robot.setActivelyShooting(false); autonomousStep += 1; } break; - case 7://reset + case 5://reset PIDDriveStraight.reset(); PIDDriveStraight.enableContinuousInput(-180,180); startDistance = robot.getDriveDistanceInchesLeft(); autonomousStep +=1; break; - case 8: //turn to go to Ball Terminal + case 6: //turn to go to Ball Terminal correction = PIDPivot.calculate(angleC + robot.getYaw() - startingYaw ); leftpower = correction; rightpower = -correction; @@ -180,7 +171,7 @@ public void autonomousPeriodic(GenericRobot robot) { startTime = System.currentTimeMillis(); } break; - case 9: //drive towards Ball Terminal + case 7: //drive towards Ball Terminal correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); @@ -199,12 +190,12 @@ public void autonomousPeriodic(GenericRobot robot) { startTime = System.currentTimeMillis(); } break; - case 10: //collect Ball Terminal + case 8: //collect Ball Terminal leftpower = 0; rightpower = 0; autonomousStep += 1; break; - case 11: //Drive forward a better shooting position + case 9: //Drive forward a better shooting position correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); leftpower = -1*(defaultPower - correction); @@ -222,20 +213,20 @@ public void autonomousPeriodic(GenericRobot robot) { autonomousStep += 1; } break; - case 12: //shoot it :) + case 10: //shoot it :) if (robot.canShoot()){ robot.setActivelyShooting(true); startTime = System.currentTimeMillis(); autonomousStep += 1.00; } break; - case 13: //shoot part 2 + case 11: //shoot part 2 if (System.currentTimeMillis() - startTime >= 250){ robot.setActivelyShooting(false); autonomousStep += 1.0; } break; - case 14: //End of autonomous, wait for Teleop + case 12: //End of autonomous, wait for Teleop leftpower = 0; rightpower = 0; break; diff --git a/src/main/java/frc/robot/generic/Lightning.java b/src/main/java/frc/robot/generic/Lightning.java index 5885566..84a7618 100644 --- a/src/main/java/frc/robot/generic/Lightning.java +++ b/src/main/java/frc/robot/generic/Lightning.java @@ -83,6 +83,8 @@ 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); @@ -249,7 +251,8 @@ public void getCargo(){ @Override public void shoot(){ double shooterRPM = defaultShooterTargetRPM; - if (getShooterRPMTop() >= shooterRPM && getShooterRPMBottom() >= shooterRPM){ + setShooterRPM(shooterRPM, shooterRPM); + if (getShooterRPMTop() >= (shooterRPM-300) && getShooterRPMBottom() >= (shooterRPM-300)){ canShoot = true; } else{ @@ -332,11 +335,11 @@ public void setTurretAngleAbsolute() { @Override public void setTurretPowerPct(double powerPct) { - if ( (getAlternateTurretAngleDegrees()>355) & (powerPct>0)) + if ( (getAlternateTurretAngle()>350) & (powerPct<0)) { powerPct = 0; } - if ( (getAlternateTurretAngleDegrees()>5) & (powerPct<0)) + if ( (getAlternateTurretAngle()<10) & (powerPct>0)) { powerPct = 0; } @@ -576,17 +579,17 @@ public double getPIDmaneuverD() { @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 @@ -605,17 +608,17 @@ public void resetAttitude() { @Override public double turretPIDgetP() { - return GenericRobot.super.turretPIDgetP(); + return 3.0e-2; } @Override public double turretPIDgetI() { - return GenericRobot.super.turretPIDgetI(); + return 0; } @Override public double turretPIDgetD() { - return GenericRobot.super.turretPIDgetD(); + return 3.0e-4; } @@ -634,6 +637,11 @@ public boolean isActivelyShooting(){ } //TODO: Add check using isReadyToShoot() function? + @Override + public boolean isReadyToShoot(){ + return true; + } + @Override public void setActivelyShooting(boolean isShooting){ isActivelyShooting = isShooting; From defb92ef5d271ee6be032c5d508fc83ef5205061 Mon Sep 17 00:00:00 2001 From: Malti Date: Sat, 26 Feb 2022 12:42:18 -0500 Subject: [PATCH 43/72] fixed auto routines to only shoot at desired position. Added in changes from testing. --- .../frc/robot/autonomous/BallBtoTerminal.java | 49 +++++++--------- .../autonomous/BallBtoTerminalReturn.java | 46 ++++++--------- .../frc/robot/autonomous/BallCtoTerminal.java | 10 ++-- .../java/frc/robot/autonomous/autoArc.java | 56 ++++++------------- 4 files changed, 61 insertions(+), 100 deletions(-) diff --git a/src/main/java/frc/robot/autonomous/BallBtoTerminal.java b/src/main/java/frc/robot/autonomous/BallBtoTerminal.java index 245892e..032b0e5 100644 --- a/src/main/java/frc/robot/autonomous/BallBtoTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallBtoTerminal.java @@ -41,9 +41,11 @@ public void autonomousInit(GenericRobot robot) { @Override public void autonomousPeriodic(GenericRobot robot) { - robot.getCargo(); - robot.shoot(); - robot.setTurretPitchPosition(.38); + if (autonomousStep >= 1){ + robot.getCargo(); + robot.shoot(); + robot.setTurretPitchPosition(.38); + } switch(autonomousStep){ case 0: //reset robot.lowerCollector(); @@ -54,23 +56,10 @@ public void autonomousPeriodic(GenericRobot robot) { if (System.currentTimeMillis() - startTime > 100){ startDistance = robot.getDriveDistanceInchesLeft(); startingYaw = robot.getYaw(); - autonomousStep = 4; - } - break; - case 1: //shoot the ball - if (robot.canShoot()){ - robot.setActivelyShooting(true); - startTime = System.currentTimeMillis(); - autonomousStep += 1; - } - break; - case 2: //shoot the ball part 2 electric boogaloo - if (System.currentTimeMillis() - startTime >= 250){ - robot.setActivelyShooting(false); autonomousStep += 1; } break; - case 3: //drive to ball B + case 1: //drive to ball B correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); leftpower = defaultPower + correction; @@ -87,34 +76,34 @@ public void autonomousPeriodic(GenericRobot robot) { startTime = System.currentTimeMillis(); } break; - case 4: //stop + case 2: //stop leftpower = 0; rightpower = 0; autonomousStep += 1; break; - case 5: + case 3: if (robot.canShoot()){ robot.setActivelyShooting(true); startTime = System.currentTimeMillis(); autonomousStep += 1.0; } break; - case 6: // part 2 not electric nor boogaloo - if (System.currentTimeMillis() - startTime >= 500){ + case 4: // part 2 not electric nor boogaloo + if (System.currentTimeMillis() - startTime >= 1000){ robot.setActivelyShooting(false); autonomousStep += 1; } break; - case 7://reset - if (System.currentTimeMillis() - startTime >= 1000){ - PIDDriveStraight.reset(); - PIDDriveStraight.enableContinuousInput(-180,180); - startDistance = robot.getDriveDistanceInchesLeft(); - autonomousStep +=1; - } + case 5://reset + + PIDDriveStraight.reset(); + PIDDriveStraight.enableContinuousInput(-180,180); + startDistance = robot.getDriveDistanceInchesLeft(); + autonomousStep +=1; + break; - case 8://drive to ball at terminal + case 6://drive to ball at terminal correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); leftpower = defaultPower + correction; @@ -131,7 +120,7 @@ public void autonomousPeriodic(GenericRobot robot) { rightpower = 0; } break; - case 9: + case 7: leftpower = 0; rightpower = 0; break; diff --git a/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java b/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java index bdf5e06..6b9b149 100644 --- a/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java +++ b/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java @@ -41,9 +41,11 @@ public void autonomousInit(GenericRobot robot) { @Override public void autonomousPeriodic(GenericRobot robot) { - robot.getCargo(); - robot.shoot(); - robot.setTurretPitchPosition(.38); + if (autonomousStep >= 1){ + robot.getCargo(); + robot.shoot(); + robot.setTurretPitchPosition(.38); + } switch(autonomousStep){ case 0: //reset robot.lowerCollector(); @@ -54,23 +56,11 @@ public void autonomousPeriodic(GenericRobot robot) { if (System.currentTimeMillis() - startTime > 100){ startDistance = robot.getDriveDistanceInchesLeft(); startingYaw = robot.getYaw(); - autonomousStep = 4; - } - break; - case 1: //shoot the ball - if (robot.canShoot()){ - robot.setActivelyShooting(true); - startTime = System.currentTimeMillis(); - autonomousStep += 1; - } - break; - case 2: //shoot the ball part 2 electric boogaloo - if (System.currentTimeMillis() - startTime >= 250){ - robot.setActivelyShooting(false); autonomousStep += 1; } break; - case 3: //drive to ball B + + case 1: //drive to ball B correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); leftpower = defaultPower + correction; @@ -87,26 +77,26 @@ public void autonomousPeriodic(GenericRobot robot) { startTime = System.currentTimeMillis(); } break; - case 4: //stop + case 2: //stop leftpower = 0; rightpower = 0; autonomousStep += 1; break; - case 5: + case 3: if (robot.canShoot()){ robot.setActivelyShooting(true); startTime = System.currentTimeMillis(); autonomousStep += 1.0; } break; - case 6: // part 2 not electric nor boogaloo - if (System.currentTimeMillis() - startTime >= 500){ + case 4: // part 2 not electric nor boogaloo + if (System.currentTimeMillis() - startTime >= 1000){ robot.setActivelyShooting(false); autonomousStep += 1; } break; - case 7://reset + case 5://reset if (System.currentTimeMillis() - startTime >= 1000){ PIDDriveStraight.reset(); PIDDriveStraight.enableContinuousInput(-180,180); @@ -114,7 +104,7 @@ public void autonomousPeriodic(GenericRobot robot) { autonomousStep +=1; } break; - case 8://drive to ball at terminal + case 6://drive to ball at terminal correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); leftpower = defaultPower + correction; @@ -131,13 +121,13 @@ public void autonomousPeriodic(GenericRobot robot) { rightpower = 0; } break; - case 9: + case 7: leftpower = 0; rightpower = 0; autonomousStep += 1.0; break; - case 10: + case 8: correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); leftpower = -defaultPower + correction; @@ -154,19 +144,19 @@ public void autonomousPeriodic(GenericRobot robot) { rightpower = 0; } break; - case 11: + case 9: if (robot.canShoot()){ robot.setActivelyShooting(true); startTime = System.currentTimeMillis(); autonomousStep += 1.00; } break; - case 12: + case 10: if (System.currentTimeMillis() - startTime >= 251){ robot.setActivelyShooting(false); autonomousStep += 1; } - case 13: + case 11: leftpower = 0; rightpower = 0; break; diff --git a/src/main/java/frc/robot/autonomous/BallCtoTerminal.java b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java index 97e083a..96e0ced 100644 --- a/src/main/java/frc/robot/autonomous/BallCtoTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java @@ -77,9 +77,11 @@ public void autonomousPeriodic(GenericRobot robot) { - robot.getCargo(); - robot.shoot(); - robot.setTurretPitchPosition(.38); + if (autonomousStep >= 1){ + robot.getCargo(); + robot.shoot(); + robot.setTurretPitchPosition(.38); + } switch(autonomousStep){ case 0: //reset robot.lowerCollector(); @@ -129,7 +131,7 @@ public void autonomousPeriodic(GenericRobot robot) { leftpower = 0; rightpower = 0; startDistance = robot.getDriveDistanceInchesLeft(); - autonomousStep = 12; + autonomousStep += 1; time = false; break; case 6: //turret turn diff --git a/src/main/java/frc/robot/autonomous/autoArc.java b/src/main/java/frc/robot/autonomous/autoArc.java index a5f95e3..33e7780 100644 --- a/src/main/java/frc/robot/autonomous/autoArc.java +++ b/src/main/java/frc/robot/autonomous/autoArc.java @@ -65,9 +65,11 @@ public void autonomousInit(GenericRobot robot) { @Override public void autonomousPeriodic(GenericRobot robot) { - robot.getCargo(); - robot.shoot(); - robot.setTurretPitchPosition(.38); + if (autonomousStep >= 1){ + robot.getCargo(); + robot.shoot(); + robot.setTurretPitchPosition(.38); + } switch (autonomousStep){ case 0: //reset robot.lowerCollector(); @@ -83,20 +85,7 @@ public void autonomousPeriodic(GenericRobot robot) { autonomousStep += 1; } break; - case 1: - if (robot.canShoot()){ - startingTime = System.currentTimeMillis(); - robot.setActivelyShooting(true); - autonomousStep += 1; - } - break; - case 2: - if (System.currentTimeMillis() - startingTime >= 250){ - robot.setActivelyShooting(false); - autonomousStep += 1; - } - break; - case 3: //straightaway + case 1: //straightaway currentYaw = robot.getYaw(); currentDistInches = robot.getDriveDistanceInchesLeft(); correction = PIDSteering.calculate(currentYaw - startYaw); @@ -108,40 +97,32 @@ public void autonomousPeriodic(GenericRobot robot) { rightPower = a; } if (currentDistInches - startInches >= rollout){ - defaultPower = .4; leftPower = 0; rightPower = 0; autonomousStep += 1; } break; - case 4: + case 2: if (robot.canShoot()){ startingTime = System.currentTimeMillis(); robot.setActivelyShooting(true); autonomousStep += 1.0; } break; - case 5: - if (System.currentTimeMillis() - startingTime >= 500){ + case 3: + if (System.currentTimeMillis() - startingTime >= 1000){ robot.setActivelyShooting(true); robot.setShooterTargetRPM(500); //super low power to yeet out undesirable cargo autonomousStep += 1; } break; - case 6: // Pid pivot 90 degrees ccw + case 4: // Pid pivot 90 degrees ccw currentYaw = robot.getYaw(); correction = PIDPivot.calculate(pivotDeg + currentYaw - startYaw); leftPower = correction; rightPower = -correction; //TODO: on lightning, change to abs encoder if (!robot.isTargetFound() && !breakRobot) { - if (!startTimer) { - startingTime = System.currentTimeMillis(); - startTimer = true; - } - currentTurretPower = .05; - } - if (System.currentTimeMillis() - startingTime > 1000 && !breakRobot){ - currentTurretPower = 0; + startingTime = System.currentTimeMillis(); breakRobot = true; } @@ -163,7 +144,7 @@ public void autonomousPeriodic(GenericRobot robot) { } break; - case 7: //Pid reset + case 5: //Pid reset PIDSteering.reset(); PIDPivot.reset(); PIDSteering.disableContinuousInput(); @@ -173,7 +154,7 @@ public void autonomousPeriodic(GenericRobot robot) { autonomousStep += 1; break; - case 8: //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)); @@ -186,17 +167,17 @@ public void autonomousPeriodic(GenericRobot robot) { autonomousStep += 1; } if (currentDistInches - startInches >= outerArcDist - 24){ - robot.setShooterTargetRPM(5000); + robot.setShooterTargetRPM(5000); //set it back to where we want robot.setActivelyShooting(false); } break; - case 9: //stop + case 7: //stop leftPower = 0; rightPower = 0; autonomousStep += 1; break; - case 10: //shoot last cargo + case 8: //shoot last cargo if (robot.canShoot()){ robot.setActivelyShooting(true); } @@ -211,14 +192,13 @@ public void autonomousPeriodic(GenericRobot robot) { counter++; } - double average = 0; + average = 0; for(double i: averageX){ average += i; } average /= averageTurretXSize; - double currentTurretPower = 0; - + currentTurretPower = 0; if(robot.isTargetFound()){ currentTurretPower = turretPIDController.calculate(average); }else{ From 8cfa1476018b3a278f72b51d410f0d1cdf53200a Mon Sep 17 00:00:00 2001 From: DeathDIVer Date: Sat, 26 Feb 2022 14:50:07 -0500 Subject: [PATCH 44/72] changed somethings in BallA and made some fixes to the 2 CtoTerminal autonomous --- src/main/java/frc/robot/Robot.java | 4 +- .../frc/robot/autonomous/BallAtoTerminal.java | 90 ++++++++++++++----- .../frc/robot/autonomous/BallCtoTerminal.java | 5 +- .../autonomous/BallCtoTerminalReturn.java | 3 - 4 files changed, 69 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3f49d2d..0776dc2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -28,10 +28,10 @@ public class Robot extends TimedRobot { simpleCTerminal = new BallCtoTerminal(), CTerminalReturn = new BallCtoTerminalReturn(); - GenericRobot robot = new Lightning(); + GenericRobot robot = new TurretBot(); Joystick joystick = new Joystick(0); Joystick xbox = new Joystick(1); - GenericAutonomous autonomous = autoArc; + GenericAutonomous autonomous = simpleATerminal; int averageTurretXSize = 2; diff --git a/src/main/java/frc/robot/autonomous/BallAtoTerminal.java b/src/main/java/frc/robot/autonomous/BallAtoTerminal.java index 247a7db..67884fb 100644 --- a/src/main/java/frc/robot/autonomous/BallAtoTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallAtoTerminal.java @@ -13,16 +13,18 @@ public class BallAtoTerminal extends GenericAutonomous { double leftpower; double rightpower; double defaultPower = .4; - double defaultTurnPower = .4; double correction; + boolean time = false; + boolean readyToShoot = false; + double shooterTargetRPM; - double distanceA = 40.44; + double distanceA = 44.2; double distanceTerminal = 259.26; - double angleA = 87.74; + double angleA = 88.74; double rampDownDist = 10; PIDController PIDDriveStraight; - + PIDController PIDPivot; TurretTracker tracker = new TurretTracker(); @Override @@ -30,7 +32,10 @@ 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); } @@ -38,44 +43,87 @@ public void autonomousInit(GenericRobot robot) { public void autonomousPeriodic(GenericRobot robot) { tracker.turretUpdate(robot); + robot.getCargo(); //collector is running + robot.shoot(); + robot.setTurretPitchPosition(.38); //change maybe 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 = 4; + autonomousStep += 1; 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 + case 1: //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; + 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 5: //stop + case 2: //stop leftpower = 0; rightpower = 0; if (System.currentTimeMillis() - startTime > 1000){ - autonomousStep = 12; + 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 >= 250){ + robot.setActivelyShooting(false); + autonomousStep += 1; + } + break; + case 5: //reset + + 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 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 @@ -83,25 +131,19 @@ public void autonomousPeriodic(GenericRobot robot) { case 11: //copium //will change these comments when they actually mean something case 12: //turn to go to ball @ terminal - 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 + //SKIPPED RIGHT NOW 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; + double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist, + robot.getDriveDistanceInchesLeft(), distanceTerminal); + leftpower = ramp; + rightpower = ramp; } if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal) { autonomousStep += 1; diff --git a/src/main/java/frc/robot/autonomous/BallCtoTerminal.java b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java index 97e083a..515d5d6 100644 --- a/src/main/java/frc/robot/autonomous/BallCtoTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallCtoTerminal.java @@ -74,9 +74,6 @@ public void autonomousPeriodic(GenericRobot robot) { turretv = tv.getDouble(0.0); //tracker.turretUpdate(robot); - - - robot.getCargo(); robot.shoot(); robot.setTurretPitchPosition(.38); @@ -108,7 +105,7 @@ public void autonomousPeriodic(GenericRobot robot) { autonomousStep += 1; } break; - case 3: //drive to ball A + case 3: //drive to ball C correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); leftpower = defaultPower + correction; diff --git a/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java index 23f227d..c054c1b 100644 --- a/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java +++ b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java @@ -78,9 +78,6 @@ public void autonomousPeriodic(GenericRobot robot) { turretv = tv.getDouble(0.0); //tracker.turretUpdate(robot); - - - robot.getCargo(); robot.shoot(); switch(autonomousStep){ From 0a8002acb340d7a1c607e06ece20f5f0398287d1 Mon Sep 17 00:00:00 2001 From: Malti Date: Sat, 26 Feb 2022 21:33:52 -0500 Subject: [PATCH 45/72] Merged autos with shoot and collector code implemented. --- .../frc/robot/autonomous/BallAtoTerminal.java | 24 +-- .../autonomous/BallAtoTerminalReturn.java | 194 ++++++++++++++++++ 2 files changed, 204 insertions(+), 14 deletions(-) create mode 100644 src/main/java/frc/robot/autonomous/BallAtoTerminalReturn.java diff --git a/src/main/java/frc/robot/autonomous/BallAtoTerminal.java b/src/main/java/frc/robot/autonomous/BallAtoTerminal.java index 67884fb..a2db35f 100644 --- a/src/main/java/frc/robot/autonomous/BallAtoTerminal.java +++ b/src/main/java/frc/robot/autonomous/BallAtoTerminal.java @@ -43,9 +43,11 @@ public void autonomousInit(GenericRobot robot) { public void autonomousPeriodic(GenericRobot robot) { tracker.turretUpdate(robot); - robot.getCargo(); //collector is running - robot.shoot(); - robot.setTurretPitchPosition(.38); //change maybe + if (autonomousStep >= 1){ + robot.getCargo(); + robot.shoot(); + robot.setTurretPitchPosition(.38); + } switch(autonomousStep){ case 0: //reset robot.lowerCollector(); @@ -93,13 +95,14 @@ public void autonomousPeriodic(GenericRobot robot) { } break; case 4: //turn shooter off - if (System.currentTimeMillis()-startTime >= 250){ + 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; @@ -124,15 +127,8 @@ public void autonomousPeriodic(GenericRobot robot) { startTime = System.currentTimeMillis(); } break; - 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 @ terminal - case 13: //drive towards the ball + case 7: //drive towards the ball //SKIPPED RIGHT NOW correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); @@ -151,7 +147,7 @@ public void autonomousPeriodic(GenericRobot robot) { rightpower = 0; } break; - case 14: + case 8: leftpower = 0; rightpower = 0; break; 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..241157f --- /dev/null +++ b/src/main/java/frc/robot/autonomous/BallAtoTerminalReturn.java @@ -0,0 +1,194 @@ +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 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; + + 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; + startDistance = robot.getDriveDistanceInchesLeft(); + autonomousStep += 1; + case 9: // drive back + 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(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 >= 250){ + robot.setActivelyShooting(false); + autonomousStep += 1; + } + break; + case 12: + leftpower = 0; + rightpower = 0; + break; + } + robot.drivePercent(leftpower, rightpower); + tracker.turretMove(robot); + + } +} From e136184c9b577c78600b1cf65ad01a4017f87e65 Mon Sep 17 00:00:00 2001 From: Malti Date: Sat, 26 Feb 2022 21:37:01 -0500 Subject: [PATCH 46/72] robot was accidentally turret, changed to lightning. --- src/main/java/frc/robot/Robot.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 462b9c9..7a2b533 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -31,7 +31,7 @@ public class Robot extends TimedRobot { CTerminalReturn = new BallCtoTerminalReturn(), simpleB = new BallSimpleB(); - GenericRobot robot = new TurretBot(); + GenericRobot robot = new Lightning(); Joystick joystick = new Joystick(0); GenericCommand command = new Hang(); Joystick xbox = new Joystick(1); @@ -293,7 +293,7 @@ public void teleopPeriodic() { average += averageX[i]; average /= averageTurretXSize; - if ((xbox.getRawAxis(2) > 0.10) & robot.isTargetFound()) { + if ((xbox.getRawAxis(2) > 0.10) & robot.isTargetFound()) { ////////////AUTO-AIM turretPower = turretPIDController.calculate(average); } else { turretPIDController.reset(); From e361a589f98a7ae5cb0f36789fda0ce4986193fa Mon Sep 17 00:00:00 2001 From: DeathDIVer Date: Sun, 27 Feb 2022 15:06:04 -0500 Subject: [PATCH 47/72] changes were made :O, actuator is being weirdchamp :( crying rn --- .idea/misc.xml | 2 +- src/main/java/frc/robot/Robot.java | 33 +++++---- .../frc/robot/autonomous/Calibration.java | 71 +++++++++++++++++++ .../frc/robot/command/GenericCommand.java | 5 ++ src/main/java/frc/robot/command/Hang.java | 53 +++++++++----- .../frc/robot/command/HangWithoutAlign.java | 4 +- src/main/java/frc/robot/generic/Camoelot.java | 4 +- src/main/java/frc/robot/generic/Falcon.java | 11 ++- .../java/frc/robot/generic/GenericRobot.java | 18 +++-- .../java/frc/robot/generic/Lightning.java | 34 ++++++--- .../java/frc/robot/generic/TurretBot.java | 13 ++-- 11 files changed, 180 insertions(+), 68 deletions(-) create mode 100644 src/main/java/frc/robot/autonomous/Calibration.java diff --git a/.idea/misc.xml b/.idea/misc.xml index 1c80b4e..7c92700 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -4,5 +4,5 @@ - + \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7a2b533..4668257 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -29,13 +29,14 @@ public class Robot extends TimedRobot { simpleBTerminal = new BallBtoTerminal(), simpleCTerminal = new BallCtoTerminal(), CTerminalReturn = new BallCtoTerminalReturn(), - simpleB = new BallSimpleB(); + simpleB = new BallSimpleB(), + calibration = new Calibration(); GenericRobot robot = new Lightning(); Joystick joystick = new Joystick(0); GenericCommand command = new Hang(); Joystick xbox = new Joystick(1); - GenericAutonomous autonomous = simpleATerminal; + GenericAutonomous autonomous = calibration; int averageTurretXSize = 2; @@ -83,6 +84,7 @@ public class Robot extends TimedRobot { @Override public void robotInit() { + robot.setTurretPitchPosition(0); } @Override @@ -122,8 +124,11 @@ public void robotPeriodic() { 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()); @@ -190,8 +195,8 @@ public void robotPeriodic() { SmartDashboard.putNumber("Autonomous Step", autonomous.autonomousStep); SmartDashboard.putString("Autonomous Program", autonomous.getClass().getName()); - SmartDashboard.putNumber("leftEncoderRaw", robot.encoderTicksLeftDrive()); - SmartDashboard.putNumber("rightEncoderRaw", robot.encoderTicksRightDrive()); + 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()); @@ -220,6 +225,7 @@ public void autonomousPeriodic() { @Override public void teleopInit() { + robot.setTurretPitchPosition(0); turretPIDController = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD()); hang = false; count = 0; @@ -231,17 +237,17 @@ public void teleopPeriodic() { switch (POVDirection.getDirection(xbox.getPOV())) { - case NORTH: - targetRPM = 5500; + case NORTH: //MEDIUM SHOT RANGE + targetRPM = 4000; turretPitch = 0.38; break; case EAST: targetRPM = 4000; turretPitch = 0.00; break; - case SOUTH: - targetRPM = 5000; - turretPitch = 0.00; + case SOUTH: ////CLOSE SHOT + targetRPM = 3500; + turretPitch = 0.12; break; case WEST: targetRPM = 5500; @@ -359,11 +365,11 @@ public void teleopPeriodic() { if (rJoyRY > -deadzone && rJoyRY < deadzone) rJoyRY = 0; if (rJoyRY > deadzone) { - turretPitch = robot.getTurretPitchPosition() + .02; + turretPitch = robot.getTurretPitchPosition() + .002; } if (rJoyRY < -deadzone) { - turretPitch = robot.getTurretPitchPosition() - .02; + turretPitch = robot.getTurretPitchPosition() - .002; } @@ -451,6 +457,7 @@ public void disabledPeriodic() { if (joystick.getRawButton(7)) autonomous = simpleCTerminal; if (joystick.getRawButton(9)) autonomous = CTerminalReturn; if (joystick.getRawButton(11)) autonomous = simpleB; + if (joystick.getRawButton(12)) autonomous = calibration; } 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/command/GenericCommand.java b/src/main/java/frc/robot/command/GenericCommand.java index ed28f0e..5b87ecd 100644 --- a/src/main/java/frc/robot/command/GenericCommand.java +++ b/src/main/java/frc/robot/command/GenericCommand.java @@ -18,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 index 36cc0ea..d60f3ce 100644 --- a/src/main/java/frc/robot/command/Hang.java +++ b/src/main/java/frc/robot/command/Hang.java @@ -16,7 +16,7 @@ public class Hang extends GenericCommand{ double startDistance; double differenceDistance; - double sensorDist = 12.0; + double sensorDist = 21.0; double Tapetheta = 0; double correction; @@ -29,7 +29,7 @@ public class Hang extends GenericCommand{ double lTraveled; - double fwd = 71.6; + double fwd = 49.5; PIDController PIDSteering; boolean tapeAlign; @@ -43,8 +43,8 @@ public class Hang extends GenericCommand{ int countRight = 0; double leftArmPower = 0; double rightArmPower = 0; - double defaultClimbPowerUp = -.5; - double defaultClimbPowerDown = .5; + double defaultClimbPowerUp = .5; + double defaultClimbPowerDown = -.5; boolean leftArrived = false; boolean rightArrived = false; double startHeightLeft = 0; @@ -57,7 +57,7 @@ public void begin(GenericRobot robot){ leftSensor = false; rightSensor = false; lTraveled = 0; - fwd = 71.6; + fwd = 49.5; PIDSteering = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD()); tapeAlign = true; firstTime = true; @@ -67,8 +67,8 @@ public void step(GenericRobot robot){ SmartDashboard.putNumber("tapetheta", Tapetheta); SmartDashboard.putNumber("ltraveled", lTraveled); SmartDashboard.putNumber("fwd", fwd); - SmartDashboard.putNumber("leftEncoderRaw", robot.encoderTicksLeftDrive()); - SmartDashboard.putNumber("rightEncoderRaw", robot.encoderTicksRightDrive()); + 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()); @@ -94,7 +94,7 @@ public void step(GenericRobot robot){ startDistance = robot.getDriveDistanceInchesLeft(); PIDSteering.reset(); PIDSteering.enableContinuousInput(-180, 180); - commandStep = 12;//TODO:change back + commandStep += 1;//TODO:change back break; case 12: //TODO: tester case, remove when necessary leftPower = defaultPower; @@ -110,7 +110,11 @@ public void step(GenericRobot robot){ leftPower = defaultPower + correction; //didn't we stop doing this? rightPower = defaultPower - correction; - if (!robot.getFloorSensorLeft()) { + if (!robot.getFloorSensorLeft() && !robot.getFloorSensorRight()){ + Tapetheta = 0; + commandStep = 3; + } + else if (!robot.getFloorSensorLeft()) { startDistance = robot.getDriveDistanceInchesLeft(); leftSensor = true; commandStep += 1; @@ -127,16 +131,17 @@ public void step(GenericRobot robot){ if (!rightSensor && !robot.getFloorSensorRight()) { differenceDistance = Math.abs(robot.getDriveDistanceInchesLeft() - startDistance); - Tapetheta = Math.atan(differenceDistance / sensorDist) * 180 / Math.PI; + 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; + 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 { @@ -151,6 +156,11 @@ public void step(GenericRobot robot){ 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; @@ -160,10 +170,11 @@ public void step(GenericRobot robot){ case 5: //adios amigos leftPower = 0; rightPower = 0; - //tapeAlign = false; TODO: make this stop being a comment - //commandStep = -1; + tapeAlign = false; + commandStep = -1; startingTime = System.currentTimeMillis(); break; + /////////TODO:Put arms forward } robot.drivePercent(leftPower, rightPower); } @@ -180,12 +191,12 @@ public void step(GenericRobot robot){ countLeft = 0; countRight = 0; if (System.currentTimeMillis() - startingTime >= 50){ - commandStep += 1; + commandStep = 2; ///TODO: fix numbering } break; case 1: ///////////unlock rotation piston to send arms forward - robot.setArmsForward(); + robot.setArmsForward(); //TODO: skip step commandStep += 1; break; case 2: //////raise climber arms (skip 10 steps after in case we need to scoot/scoot @@ -222,14 +233,17 @@ public void step(GenericRobot robot){ countRight = 0; leftArmPower = 0; rightArmPower = 0; + startingTime = System.currentTimeMillis(); commandStep += 1; } break; - case 3: ///////////unlock rotation piston to send arms forward + case 3: ///////////unlock rotation piston to send arms back robot.setArmsBackward(); - commandStep = 11; + if (System.currentTimeMillis() - startingTime >= 500) { + commandStep = 11; + } break; /*case 2: //////disable PTO robot.turnOffPTO(); @@ -362,6 +376,7 @@ public void step(GenericRobot robot){ rightArmPower = 0; leftArmPower = 0; commandStep = 16; //////////skip over step 15 + startingTime = System.currentTimeMillis(); } break; case 15:///change to check with pitch and roll @@ -380,7 +395,9 @@ public void step(GenericRobot robot){ } case 16://///////once in contact move arms back again with the piston and swiiiiing robot.setArmsBackward(); - commandStep += 1;//TODO:change back + if (System.currentTimeMillis() - startingTime >= 500) { + commandStep += 1;//TODO:change back + } break; case 17://////////go back to case 11 and repeat down to this step if (firstTime){ diff --git a/src/main/java/frc/robot/command/HangWithoutAlign.java b/src/main/java/frc/robot/command/HangWithoutAlign.java index 331521b..d152829 100644 --- a/src/main/java/frc/robot/command/HangWithoutAlign.java +++ b/src/main/java/frc/robot/command/HangWithoutAlign.java @@ -67,8 +67,8 @@ public void step(GenericRobot robot){ SmartDashboard.putNumber("tapetheta", Tapetheta); SmartDashboard.putNumber("ltraveled", lTraveled); SmartDashboard.putNumber("fwd", fwd); - SmartDashboard.putNumber("leftEncoderRaw", robot.encoderTicksLeftDrive()); - SmartDashboard.putNumber("rightEncoderRaw", robot.encoderTicksRightDrive()); + 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()); 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 8524f1e..b3f2094 100644 --- a/src/main/java/frc/robot/generic/GenericRobot.java +++ b/src/main/java/frc/robot/generic/GenericRobot.java @@ -41,11 +41,11 @@ 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(){ @@ -71,16 +71,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; @@ -154,7 +162,7 @@ public default void setCollectorIntakePercentage( } public default double getCollectorIntakePercentage() { return 0; - }; + } public void setIndexerIntakePercentage( double percentage diff --git a/src/main/java/frc/robot/generic/Lightning.java b/src/main/java/frc/robot/generic/Lightning.java index 84a7618..db6079b 100644 --- a/src/main/java/frc/robot/generic/Lightning.java +++ b/src/main/java/frc/robot/generic/Lightning.java @@ -7,7 +7,7 @@ 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 = 1; @@ -37,8 +37,10 @@ public class Lightning implements GenericRobot { Servo elevationRight = new Servo(1); - 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(); @@ -156,12 +158,12 @@ 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 @@ -195,13 +197,23 @@ public double encoderRightDriveTicksPerInch() { } @Override - public double encoderTicksLeftDrive() { - return encoderLeft.getPosition(); + public double encoderTicksLeftDriveA() { + return encoderLeftA.getPosition(); } @Override - public double encoderTicksRightDrive() { - return encoderRight.getPosition(); + public double encoderTicksRightDriveA() { + return encoderRightA.getPosition(); + } + + @Override + public double encoderTicksLeftDriveB(){ + return encoderLeftB.getPosition(); + } + + @Override + public double encoderTicksRightDriveB(){ + return encoderRightB.getPosition(); } @Override @@ -523,13 +535,13 @@ public void lowerClimberArms(double rightPower, double leftPower){ public double armHeightLeft() { //TODO: put in conversion //Maybe use some sensor. Do NOT want to use encoders for this. - return -encoderTicksLeftDrive()*.24937; + return -encoderTicksLeftDriveA()*.24937; } @Override public double armHeightRight(){ //TODO: put in conversion - return -encoderTicksRightDrive()*.24937; + return -encoderTicksRightDriveA()*.24937; } @Override 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(); } From ef342782bb20d5444f8b8ff5da0c2af1be995708 Mon Sep 17 00:00:00 2001 From: DeathDIVer Date: Sun, 27 Feb 2022 17:14:25 -0500 Subject: [PATCH 48/72] Changes from auto testing of BallBtoTerminalReturn. TODO:implement the changes into other routines --- src/main/java/frc/robot/Robot.java | 4 ++-- .../autonomous/BallBtoTerminalReturn.java | 22 +++++++++---------- .../java/frc/robot/generic/Lightning.java | 6 ++--- 3 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4668257..66f4d7b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -29,6 +29,7 @@ public class Robot extends TimedRobot { simpleBTerminal = new BallBtoTerminal(), simpleCTerminal = new BallCtoTerminal(), CTerminalReturn = new BallCtoTerminalReturn(), + BTerminalReturn = new BallBtoTerminalReturn(), simpleB = new BallSimpleB(), calibration = new Calibration(); @@ -36,7 +37,7 @@ public class Robot extends TimedRobot { Joystick joystick = new Joystick(0); GenericCommand command = new Hang(); Joystick xbox = new Joystick(1); - GenericAutonomous autonomous = calibration; + GenericAutonomous autonomous = BTerminalReturn; int averageTurretXSize = 2; @@ -457,7 +458,6 @@ public void disabledPeriodic() { if (joystick.getRawButton(7)) autonomous = simpleCTerminal; if (joystick.getRawButton(9)) autonomous = CTerminalReturn; if (joystick.getRawButton(11)) autonomous = simpleB; - if (joystick.getRawButton(12)) autonomous = calibration; } diff --git a/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java b/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java index 6b9b149..75994e3 100644 --- a/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java +++ b/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java @@ -83,26 +83,26 @@ public void autonomousPeriodic(GenericRobot robot) { autonomousStep += 1; break; case 3: - if (robot.canShoot()){ + if (robot.isTargetFound() && 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){ + if (System.currentTimeMillis() - startTime >= 1500){ robot.setActivelyShooting(false); autonomousStep += 1; } break; case 5://reset - if (System.currentTimeMillis() - startTime >= 1000){ - PIDDriveStraight.reset(); - PIDDriveStraight.enableContinuousInput(-180,180); - startDistance = robot.getDriveDistanceInchesLeft(); - autonomousStep +=1; - } + + 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); @@ -124,9 +124,9 @@ public void autonomousPeriodic(GenericRobot robot) { case 7: leftpower = 0; rightpower = 0; + startDistance = robot.getDriveDistanceInchesLeft(); autonomousStep += 1.0; break; - case 8: correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); @@ -145,7 +145,7 @@ public void autonomousPeriodic(GenericRobot robot) { } break; case 9: - if (robot.canShoot()){ + if (robot.isTargetFound() && robot.canShoot()){ robot.setActivelyShooting(true); startTime = System.currentTimeMillis(); autonomousStep += 1.00; @@ -182,7 +182,7 @@ public void autonomousPeriodic(GenericRobot robot) { PIDTurret.reset(); } if((!robot.isTargetFound()) && (System.currentTimeMillis() - startTime < 2000)) { - currentTurretPower = .2; + currentTurretPower = .4; } robot.setTurretPowerPct(currentTurretPower); diff --git a/src/main/java/frc/robot/generic/Lightning.java b/src/main/java/frc/robot/generic/Lightning.java index db6079b..138d550 100644 --- a/src/main/java/frc/robot/generic/Lightning.java +++ b/src/main/java/frc/robot/generic/Lightning.java @@ -66,7 +66,7 @@ public class Lightning implements GenericRobot { SparkMaxLimitSwitch limitSwitchLeftBForward = leftMotorB.getForwardLimitSwitch(lstype); SparkMaxLimitSwitch limitSwitchLeftBReverse = leftMotorB.getReverseLimitSwitch(lstype); - double defaultShooterTargetRPM = 5000; + double defaultShooterTargetRPM = 4000; boolean isPTOonArms; @@ -100,14 +100,14 @@ public Lightning(){ 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(2.5e-4); + shooterAPIDController.setP(9.0e-4); shooterAPIDController.setI(0); shooterAPIDController.setD(2.5e-1); shooterAPIDController.setFF(1.67e-4); shooterAPIDController.getIZone(500); shooterAPIDController.getDFilter(0); - shooterBPIDController.setP(2.5e-4); + shooterBPIDController.setP(9.0e-4); shooterBPIDController.setI(0); shooterBPIDController.setD(2.5e-1); shooterBPIDController.setFF(1.67e-4); From b7564c44e3396b5a5dba6dcd7275e222e6cdfc62 Mon Sep 17 00:00:00 2001 From: DeathDIVer Date: Sun, 27 Feb 2022 22:20:15 -0500 Subject: [PATCH 49/72] update to 2022.3.1 --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index aa77ea2..59d4569 100644 --- a/build.gradle +++ b/build.gradle @@ -2,7 +2,7 @@ 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.3.1" } sourceCompatibility = JavaVersion.VERSION_11 From 0b9f44b0f4f4c2656c40bc4013175d0f8b492b9b Mon Sep 17 00:00:00 2001 From: Louminator Date: Sun, 27 Feb 2022 22:59:31 -0500 Subject: [PATCH 50/72] Added code in teleop buttons 10, 11 and 12 to set up the collector, as the last step of a flight check. --- src/main/java/frc/robot/Robot.java | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 66f4d7b..73ef54e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -409,6 +409,29 @@ public void teleopPeriodic() { curIndexer = defIndexerPower; } + /* Some code for setting up the collector after flight check. */ + if (joystick.getRawButton(10)) { + if (robot.getAlternateTurretAngle()>320) + { + turretPower = 0.4; + } + else if (robot.getAlternateTurretAngle()<310) + { + turretPower = -0.4; + } + else + { + turretPower = 0.0; + } + } + + if (joystick.getRawButton(11) & joystick.getRawButton(12) & + (Math.abs(robot.getAlternateTurretAngleDegrees()-315)<5) ) + { + robot.raiseCollector(); + } + + //////////////////////////////////////////////////COLLECTOR LOGIC ENDS From 66fff75a7941677076066b878c75be7c513a7fc7 Mon Sep 17 00:00:00 2001 From: Louminator Date: Sun, 27 Feb 2022 23:00:33 -0500 Subject: [PATCH 51/72] Lower the collector if you need to. --- .idea/misc.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.idea/misc.xml b/.idea/misc.xml index 7c92700..b808dca 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -4,5 +4,5 @@ - + \ No newline at end of file From 4975d869eeca248b7fdddda80cec55061f6dabf9 Mon Sep 17 00:00:00 2001 From: Louminator Date: Mon, 28 Feb 2022 15:56:52 -0500 Subject: [PATCH 52/72] Update Robot.java Created variables to govern the turret storage position and tolerances for it. --- src/main/java/frc/robot/Robot.java | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 73ef54e..7b844ab 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -410,12 +410,15 @@ public void teleopPeriodic() { } /* Some code for setting up the collector after flight check. */ + double flightCheckTol = 5.0; + double storagePosition = 315; + if (joystick.getRawButton(10)) { - if (robot.getAlternateTurretAngle()>320) + if (robot.getAlternateTurretAngle()>storagePosition+flightCheckTol) { turretPower = 0.4; } - else if (robot.getAlternateTurretAngle()<310) + else if (robot.getAlternateTurretAngle() Date: Mon, 28 Feb 2022 18:49:47 -0500 Subject: [PATCH 53/72] Joystick buttons 1-4 for ez driving: forward, backwards, pivots --- build.gradle | 2 +- src/main/java/frc/robot/Robot.java | 49 ++++++++++++++++++++++-------- 2 files changed, 37 insertions(+), 14 deletions(-) diff --git a/build.gradle b/build.gradle index aa77ea2..e1a1be8 100644 --- a/build.gradle +++ b/build.gradle @@ -2,7 +2,7 @@ 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 diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7a2b533..ff3306b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -11,8 +11,10 @@ import frc.robot.autonomous.*; import frc.robot.autonomous.GenericAutonomous; import frc.robot.command.*; +import frc.robot.generic.Falcon; import frc.robot.generic.GenericRobot; import frc.robot.generic.Lightning; +import frc.robot.generic.TurretBot; import java.util.Arrays; import java.util.Map; @@ -31,7 +33,7 @@ public class Robot extends TimedRobot { CTerminalReturn = new BallCtoTerminalReturn(), simpleB = new BallSimpleB(); - GenericRobot robot = new Lightning(); + GenericRobot robot = new TurretBot(); Joystick joystick = new Joystick(0); GenericCommand command = new Hang(); Joystick xbox = new Joystick(1); @@ -249,7 +251,6 @@ public void teleopPeriodic() { break; } - //note to self: buttons currently assume mirrored joystick setting if (joystick.getRawButtonPressed(8)) { count = (count + 1) % 2; } @@ -265,19 +266,41 @@ public void teleopPeriodic() { //////////////////////////////////////////////////DRIVETRAIN CONTROL - joystickX = joystick.getX(); - joystickY = -joystick.getY(); + if (!robot.getPTOState()) { - if (joystickY > -cutoff && joystickY < cutoff) { - joystickY = 0; - } - if (joystickX > -cutoff && joystickX < cutoff) { - joystickX = 0; - } + if(joystick.getRawButton(1)){ + driveLeft = 1; + driveRight = 1; + } + else if(joystick.getRawButton(2)){ + driveLeft = -1; + driveRight = -1; + } + else if(joystick.getRawButton(3)){ + //pivot left + driveLeft = -1; + driveRight = 1; + } + else if(joystick.getRawButton(4)){ + //pivot right + driveLeft = 1; + driveRight = -1; + } + 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; + } - if (!robot.getPTOState()) { - driveLeft = (joystickY + joystickX) * scaleFactor; - driveRight = (joystickY - joystickX) * scaleFactor; } //////////////////////////////////////////////DRIVETRAIN CONTROL ENDS From e185344af09b51ebb07eaea16a4bd20fa0cdd8fa Mon Sep 17 00:00:00 2001 From: Malti Date: Mon, 28 Feb 2022 19:24:29 -0500 Subject: [PATCH 54/72] Driver command merge. --- src/main/java/frc/robot/Robot.java | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6a785f3..0a1e0f9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -276,22 +276,22 @@ public void teleopPeriodic() { if (!robot.getPTOState()) { if(joystick.getRawButton(1)){ - driveLeft = 1; - driveRight = 1; + driveLeft = .4; + driveRight = .4 ; } else if(joystick.getRawButton(2)){ - driveLeft = -1; - driveRight = -1; + driveLeft = -.4; + driveRight = -.4; } else if(joystick.getRawButton(3)){ //pivot left - driveLeft = -1; - driveRight = 1; + driveLeft = -.4; + driveRight = .4; } else if(joystick.getRawButton(4)){ //pivot right - driveLeft = 1; - driveRight = -1; + driveLeft = .4; + driveRight = -.4; } else{ joystickX = joystick.getX(); From a5e5f52fbce112b4d42ead7d8ca9f3a29c32be36 Mon Sep 17 00:00:00 2001 From: Malti Date: Mon, 28 Feb 2022 21:30:57 -0500 Subject: [PATCH 55/72] Hang code and auto routine. TODO: update simpleC with B changes and add delay in climb. --- src/main/java/frc/robot/Robot.java | 12 +++- .../autonomous/BallBtoTerminalReturn.java | 64 +++++++++++-------- src/main/java/frc/robot/command/Hang.java | 28 +++++--- 3 files changed, 64 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0a1e0f9..b9247b1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -35,7 +35,7 @@ public class Robot extends TimedRobot { simpleB = new BallSimpleB(), calibration = new Calibration(); - GenericRobot robot = new TurretBot(); + GenericRobot robot = new Lightning(); Joystick joystick = new Joystick(0); GenericCommand command = new Hang(); Joystick xbox = new Joystick(1); @@ -228,7 +228,7 @@ public void autonomousPeriodic() { @Override public void teleopInit() { - robot.setTurretPitchPosition(0); + turretPitch = 0; turretPIDController = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD()); hang = false; countHang = 0; @@ -335,6 +335,12 @@ else if(joystick.getRawButton(4)){ turretPower = 0.0; } } + if (joystick.getRawButtonPressed(5)){ + turretPIDController.reset(); + } + if (joystick.getRawButton(5)){ + turretPower = -turretPIDController.calculate(robot.getAlternateTurretAngle()-45); + } /////////////////////////////////////////////////////TURRET CONTROL ENDS @@ -384,7 +390,7 @@ else if(joystick.getRawButton(4)){ ///////////////////////////////////////////////////////////ACTUATOR STUFF - double deadzone = 0.1; + double deadzone = 0.5; double rJoyRY = xbox.getRawAxis(5); if (rJoyRY > -deadzone && rJoyRY < deadzone) rJoyRY = 0; diff --git a/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java b/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java index 75994e3..990c146 100644 --- a/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java +++ b/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java @@ -13,11 +13,11 @@ public class BallBtoTerminalReturn extends GenericAutonomous { double leftpower; double rightpower; - double defaultPower = .4; + double defaultPower = .5; double correction; double distanceB = 61.5; - double distanceTerminal = 160.6; + double distanceTerminal = 154.6; double rampDownDist = 10; PIDController PIDTurret; @@ -41,11 +41,40 @@ public void autonomousInit(GenericRobot robot) { @Override public void autonomousPeriodic(GenericRobot robot) { - if (autonomousStep >= 1){ - robot.getCargo(); - robot.shoot(); + 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 + + + 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(); @@ -83,14 +112,14 @@ public void autonomousPeriodic(GenericRobot robot) { autonomousStep += 1; break; case 3: - if (robot.isTargetFound() && robot.canShoot()){ + 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 >= 1500){ + if (System.currentTimeMillis() - startTime >= 2000){ robot.setActivelyShooting(false); autonomousStep += 1; } @@ -163,28 +192,7 @@ public void autonomousPeriodic(GenericRobot robot) { } 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 = .4; - } - robot.setTurretPowerPct(currentTurretPower); } } diff --git a/src/main/java/frc/robot/command/Hang.java b/src/main/java/frc/robot/command/Hang.java index d60f3ce..455f888 100644 --- a/src/main/java/frc/robot/command/Hang.java +++ b/src/main/java/frc/robot/command/Hang.java @@ -49,6 +49,8 @@ public class Hang extends GenericCommand{ boolean rightArrived = false; double startHeightLeft = 0; double startHeightRight = 0; + double turretPower = 0; + PIDController turretPIDController; public void begin(GenericRobot robot){ @@ -59,6 +61,7 @@ public void begin(GenericRobot robot){ 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; } @@ -79,11 +82,23 @@ public void step(GenericRobot robot){ 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);*/ + robot.raiseCollector(); switch (commandStep) { /////////////tapeAlign Code case -1: robot.resetEncoders(); robot.resetAttitude(); + turretPIDController.disableContinuousInput(); if (System.currentTimeMillis() >= startingTime + 100) { commandStep += 1; @@ -190,7 +205,7 @@ else if (!robot.getFloorSensorLeft()) { robot.resetEncoders(); countLeft = 0; countRight = 0; - if (System.currentTimeMillis() - startingTime >= 50){ + if (System.currentTimeMillis() - startingTime >= 5000){ commandStep = 2; ///TODO: fix numbering } @@ -297,12 +312,7 @@ else if (!robot.getFloorSensorLeft()) { leftArmPower = 0; rightArmPower = 0; //TODO: change - if (!firstTime){ - commandStep = 19; - } - else { - commandStep = 30; - } + commandStep = 30; leftArrived = false; rightArrived = false; startHeightLeft = robot.armHeightLeft(); @@ -452,7 +462,7 @@ else if (!robot.getFloorSensorLeft()) { 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; From 25a535ebfafff06c249e857b7276bdd2332ddc7a Mon Sep 17 00:00:00 2001 From: Malti Date: Tue, 1 Mar 2022 08:19:56 -0500 Subject: [PATCH 56/72] Changes from testing autos implemented in all auto routines. NB: there is a manual climb written in the driver control which uses the same axis as the actuator. --- .../autonomous/BallBtoTerminalReturn.java | 8 +- .../autonomous/BallCtoTerminalReturn.java | 79 ++++++++----------- .../java/frc/robot/autonomous/autoArc.java | 2 +- src/main/java/frc/robot/command/Hang.java | 4 +- 4 files changed, 42 insertions(+), 51 deletions(-) diff --git a/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java b/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java index 990c146..0c5733b 100644 --- a/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java +++ b/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java @@ -66,8 +66,10 @@ public void autonomousPeriodic(GenericRobot robot) { //////////AUTO TRACK STUFF - robot.getCargo(); - robot.shoot(); + if (autonomousStep >= 1){ + robot.getCargo(); + robot.shoot(); + } if (autonomousStep >= 1 && autonomousStep <=10){ robot.setTurretPitchPosition(.38); } @@ -181,7 +183,7 @@ public void autonomousPeriodic(GenericRobot robot) { } break; case 10: - if (System.currentTimeMillis() - startTime >= 251){ + if (System.currentTimeMillis() - startTime >= 500){ robot.setActivelyShooting(false); autonomousStep += 1; } diff --git a/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java index 9579bf8..cf9b748 100644 --- a/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java +++ b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java @@ -15,8 +15,7 @@ public class BallCtoTerminalReturn extends GenericAutonomous { double leftpower; double rightpower; - double defaultPower = .4; - double defaultTurnPower = .4; + double defaultPower = .5; double correction; boolean time = false; @@ -31,10 +30,6 @@ public class BallCtoTerminalReturn extends GenericAutonomous { int averageTurretXSize = 2; double[] averageTurretX = new double [averageTurretXSize]; - double turretx; - double turrety; - double turretarea; - double turretv; int counter = 0; double shooterTargetRPM; @@ -43,7 +38,6 @@ public class BallCtoTerminalReturn extends GenericAutonomous { double indexerPct; double collectorPct; - //TurretTracker tracker = new TurretTracker(); boolean initialTurretSpin = true; @@ -60,23 +54,36 @@ public void autonomousInit(GenericRobot robot) { 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(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); + + @@ -84,8 +91,14 @@ public void autonomousPeriodic(GenericRobot robot) { 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(); @@ -128,14 +141,14 @@ public void autonomousPeriodic(GenericRobot robot) { time = false; break; case 3: //create a target shooter value and see if shooter reaches it. - if (robot.canShoot()){ + 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 >= 1000){ + if (System.currentTimeMillis() - startTime >= 2000){ robot.setActivelyShooting(false); autonomousStep += 1; } @@ -221,7 +234,7 @@ public void autonomousPeriodic(GenericRobot robot) { } break; case 11: //shoot part 2 - if (System.currentTimeMillis() - startTime >= 250){ + if (System.currentTimeMillis() - startTime >= 1000){ robot.setActivelyShooting(false); autonomousStep += 1.0; } @@ -233,30 +246,6 @@ 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 = 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/autoArc.java b/src/main/java/frc/robot/autonomous/autoArc.java index 33e7780..cbb12a6 100644 --- a/src/main/java/frc/robot/autonomous/autoArc.java +++ b/src/main/java/frc/robot/autonomous/autoArc.java @@ -103,7 +103,7 @@ public void autonomousPeriodic(GenericRobot robot) { } break; case 2: - if (robot.canShoot()){ + if (robot.canShoot() && robot.isTargetFound() &&(-5= 500) { + if (System.currentTimeMillis() - startingTime >= 1000) { commandStep = 11; } break; @@ -405,7 +405,7 @@ else if (!robot.getFloorSensorLeft()) { } case 16://///////once in contact move arms back again with the piston and swiiiiing robot.setArmsBackward(); - if (System.currentTimeMillis() - startingTime >= 500) { + if (System.currentTimeMillis() - startingTime >= 1000) { commandStep += 1;//TODO:change back } break; From e858c9e74423f2e8617ad86bd23becfa9d0d8f73 Mon Sep 17 00:00:00 2001 From: DeathDIVer Date: Tue, 1 Mar 2022 18:43:31 -0500 Subject: [PATCH 57/72] update to 2022.3.1 --- src/main/java/frc/robot/Robot.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7b844ab..5a99313 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -383,6 +383,7 @@ public void teleopPeriodic() { //button 2 = bottom center button if (xbox.getRawButton(2)) { if (!robot.getUpperCargo()) { + //consumes ball curCollector = defCollectorPower; curIndexer = defIndexerPower; } else { @@ -397,6 +398,7 @@ public void teleopPeriodic() { curIndexer = defIndexerPower; } } else if (xbox.getRawButton(4)) { + //barfs out ball curCollector = -defCollectorPower; curIndexer = -defIndexerPower; } else { From 8af88d3b8535c5310a12e9d3f786053ae4054552 Mon Sep 17 00:00:00 2001 From: Louminator Date: Tue, 1 Mar 2022 18:48:04 -0500 Subject: [PATCH 58/72] Installed offset for turrent angle. --- src/main/java/frc/robot/Robot.java | 4 ++-- src/main/java/frc/robot/generic/Lightning.java | 13 ++++++++++++- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b9247b1..4669661 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -164,8 +164,8 @@ public void robotPeriodic() { 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("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()); diff --git a/src/main/java/frc/robot/generic/Lightning.java b/src/main/java/frc/robot/generic/Lightning.java index 7e553c2..b1b0695 100644 --- a/src/main/java/frc/robot/generic/Lightning.java +++ b/src/main/java/frc/robot/generic/Lightning.java @@ -328,7 +328,18 @@ public double encoderTurretTicksPerDegree(){ @Override public double getAlternateTurretAngle(){ double raw = encoderTurretAlt.getPosition(); - return (raw * 136.467) - 5.73; + double out; + double offset = 185-45; + out = (raw * 136.467) - 5.73 - offset; + if (out>360) + { + out = out-360; + } + else if (out<0) + { + out = out+360; + } + return (out); } @Override From 67ca9527a8430f93c125ff908bf9d9489fadfe62 Mon Sep 17 00:00:00 2001 From: Malti Date: Tue, 1 Mar 2022 19:06:14 -0500 Subject: [PATCH 59/72] Changes to BallCTerminalReturn and offset start pos. --- build.gradle | 3 +-- src/main/java/frc/robot/Robot.java | 4 ++-- .../frc/robot/autonomous/BallCtoTerminalReturn.java | 12 +++++++----- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/build.gradle b/build.gradle index e1a1be8..23c46ee 100644 --- a/build.gradle +++ b/build.gradle @@ -9,8 +9,7 @@ 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 4669661..176492b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -39,7 +39,7 @@ public class Robot extends TimedRobot { Joystick joystick = new Joystick(0); GenericCommand command = new Hang(); Joystick xbox = new Joystick(1); - GenericAutonomous autonomous = BTerminalReturn; + GenericAutonomous autonomous = CTerminalReturn; int averageTurretXSize = 2; @@ -440,7 +440,7 @@ else if(joystick.getRawButton(4)){ /* Some code for setting up the collector after flight check. */ double flightCheckTol = 5.0; - double storagePosition = 312; + double storagePosition = 317; if (joystick.getRawButton(13)) { if (robot.getAlternateTurretAngle()>storagePosition+flightCheckTol) diff --git a/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java index cf9b748..f16ef29 100644 --- a/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java +++ b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java @@ -20,7 +20,7 @@ public class BallCtoTerminalReturn extends GenericAutonomous { boolean time = false; double distanceC = 47.9; - double distanceTerminal = 251; + double distanceTerminal = 225; double angleC = 82.74; double rampDownDist = 10; @@ -40,7 +40,6 @@ public class BallCtoTerminalReturn extends GenericAutonomous { - boolean initialTurretSpin = true; @Override public void autonomousInit(GenericRobot robot) { autonomousStep = 0; @@ -78,8 +77,11 @@ public void autonomousPeriodic(GenericRobot robot) { }else{ PIDTurret.reset(); } - if((!robot.isTargetFound()) && (System.currentTimeMillis() - startTime < 2000)) { - currentTurretPower = .2; + + if (autonomousStep < 4){ + if((!robot.isTargetFound()) && (System.currentTimeMillis() - startTime < 2000)) { + currentTurretPower = .2; + } } robot.setTurretPowerPct(currentTurretPower); @@ -108,7 +110,7 @@ public void autonomousPeriodic(GenericRobot robot) { PIDDriveStraight.enableContinuousInput(-180,180); robot.resetEncoders(); robot.resetAttitude(); - if (System.currentTimeMillis() - startTime > 100){ + if (System.currentTimeMillis() - startTime > 1000){ autonomousStep += 1; startingYaw = robot.getYaw(); startDistance = robot.getDriveDistanceInchesLeft(); From bdc4b074133030f86692e651410eccdf08a3d8e6 Mon Sep 17 00:00:00 2001 From: Louminator Date: Tue, 1 Mar 2022 19:58:39 -0500 Subject: [PATCH 60/72] It works!!! --- .../robot/autonomous/BallCtoTerminalReturn.java | 17 +++++++++-------- .../java/frc/robot/generic/GenericRobot.java | 4 ++++ 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java index f16ef29..4b8178b 100644 --- a/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java +++ b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java @@ -52,8 +52,7 @@ public void autonomousInit(GenericRobot robot) { 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 @@ -79,17 +78,17 @@ public void autonomousPeriodic(GenericRobot robot) { } if (autonomousStep < 4){ - if((!robot.isTargetFound()) && (System.currentTimeMillis() - startTime < 2000)) { + if((!robot.isTargetFound()) && (System.currentTimeMillis() - startTime < 5000)) { currentTurretPower = .2; } } + if ((autonomousStep>=4) && (autonomousStep < 8)){ + if((!robot.isTargetFound()) && (System.currentTimeMillis() - startTime < 5000)) { + currentTurretPower = -.2; + } + } robot.setTurretPowerPct(currentTurretPower); - - - - - if (autonomousStep >= 1){ robot.getCargo(); robot.shoot(); @@ -166,6 +165,7 @@ public void autonomousPeriodic(GenericRobot robot) { 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){ @@ -209,6 +209,7 @@ public void autonomousPeriodic(GenericRobot robot) { leftpower = 0; rightpower = 0; autonomousStep += 1; + robot.setPipeline(0); break; case 9: //Drive forward a better shooting position correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); diff --git a/src/main/java/frc/robot/generic/GenericRobot.java b/src/main/java/frc/robot/generic/GenericRobot.java index e83f9a0..44c67ba 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, From 45ee0d45f79b6005af399164eee02a55cc2ed775 Mon Sep 17 00:00:00 2001 From: Louminator Date: Tue, 1 Mar 2022 20:12:44 -0500 Subject: [PATCH 61/72] Tuned up auto... 14 secs. --- .../java/frc/robot/autonomous/BallCtoTerminalReturn.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java index 4b8178b..208f414 100644 --- a/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java +++ b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java @@ -15,7 +15,7 @@ public class BallCtoTerminalReturn extends GenericAutonomous { double leftpower; double rightpower; - double defaultPower = .5; + double defaultPower = .6; double correction; boolean time = false; @@ -79,7 +79,7 @@ public void autonomousPeriodic(GenericRobot robot) { if (autonomousStep < 4){ if((!robot.isTargetFound()) && (System.currentTimeMillis() - startTime < 5000)) { - currentTurretPower = .2; + currentTurretPower = .3; } } if ((autonomousStep>=4) && (autonomousStep < 8)){ @@ -109,7 +109,7 @@ public void autonomousPeriodic(GenericRobot robot) { PIDDriveStraight.enableContinuousInput(-180,180); robot.resetEncoders(); robot.resetAttitude(); - if (System.currentTimeMillis() - startTime > 1000){ + if (System.currentTimeMillis() - startTime > 500){ autonomousStep += 1; startingYaw = robot.getYaw(); startDistance = robot.getDriveDistanceInchesLeft(); From 1217abeef2e2b81caa683931bc740e76797ccb92 Mon Sep 17 00:00:00 2001 From: DeathDIVer Date: Tue, 1 Mar 2022 21:13:57 -0500 Subject: [PATCH 62/72] update to 2022.3.1 --- src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java index cf9b748..089ab9d 100644 --- a/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java +++ b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java @@ -114,7 +114,6 @@ public void autonomousPeriodic(GenericRobot robot) { startDistance = robot.getDriveDistanceInchesLeft(); } break; - case 1: //drive to ball C collectorPct = 1; correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); From d248820987765ece33f6b78e951facdf21f1f6dd Mon Sep 17 00:00:00 2001 From: DeathDIVer Date: Tue, 1 Mar 2022 21:35:11 -0500 Subject: [PATCH 63/72] Fixed some comments on CtoTerminalReturn and brought AtoTerminalReturn up to date --- .../autonomous/BallAtoTerminalReturn.java | 78 ++++++++++++++----- .../autonomous/BallCtoTerminalReturn.java | 4 +- 2 files changed, 61 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/autonomous/BallAtoTerminalReturn.java b/src/main/java/frc/robot/autonomous/BallAtoTerminalReturn.java index 241157f..d9082ab 100644 --- a/src/main/java/frc/robot/autonomous/BallAtoTerminalReturn.java +++ b/src/main/java/frc/robot/autonomous/BallAtoTerminalReturn.java @@ -5,6 +5,7 @@ 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; @@ -18,6 +19,13 @@ public class BallAtoTerminalReturn extends GenericAutonomous { boolean readyToShoot = false; double shooterTargetRPM; + int averageTurretXSize = 2; + double[] averageTurretX = new double [averageTurretXSize]; + int counter = 0; + + double indexerPct; + double collectorPct; + double distanceA = 44.2; double distanceTerminal = 259.26; double angleA = 88.74; @@ -37,17 +45,48 @@ public void autonomousInit(GenericRobot robot) { PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDpivotD()); PIDPivot = new PIDController(robot.getPIDpivotP(), robot.getPIDpivotI(), robot.getPIDpivotD()); tracker.turretInit(robot); + robot.setPipeline(0); } @Override public void autonomousPeriodic(GenericRobot robot) { - tracker.turretUpdate(robot); + double currentTurretPower = 0; + double average = 0; + + if(robot.isTargetFound()) { + averageTurretX[counter % averageTurretXSize] = robot.getTargetX(); + counter++; + } + for(double i: averageTurretX){ + average += i; + } + average /= averageTurretXSize; + + 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); } + + tracker.turretUpdate(robot); + switch(autonomousStep){ case 0: //reset robot.lowerCollector(); @@ -57,13 +96,14 @@ public void autonomousPeriodic(GenericRobot robot) { PIDPivot.enableContinuousInput(-180,180); robot.resetEncoders(); robot.resetAttitude(); - if (System.currentTimeMillis() - startTime > 100){ + if (System.currentTimeMillis() - startTime > 500){ autonomousStep += 1; startingYaw = robot.getYaw(); startDistance = robot.getDriveDistanceInchesLeft(); } break; case 1: //drive to ball A + collectorPct = 1; correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); leftpower = defaultPower + correction; @@ -83,30 +123,34 @@ public void autonomousPeriodic(GenericRobot robot) { case 2: //stop leftpower = 0; rightpower = 0; - if (System.currentTimeMillis() - startTime > 1000){ - autonomousStep += 1; - } + startDistance = robot.getDriveDistanceInchesLeft(); + autonomousStep += 1; + time = false; break; case 3: //shoot the ball if target is found - if (robot.canShoot()){ + 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 >= 1000){ + if (System.currentTimeMillis()-startTime >= 2000){ robot.setActivelyShooting(false); autonomousStep += 1; } break; case 5: //reset - autonomousStep += 1; + 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){ @@ -127,8 +171,7 @@ public void autonomousPeriodic(GenericRobot robot) { startTime = System.currentTimeMillis(); } break; - case 7: //drive towards the ball - //SKIPPED RIGHT NOW + case 7: //drive towards Ball Terminal correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); leftpower = defaultPower + correction; @@ -142,20 +185,20 @@ public void autonomousPeriodic(GenericRobot robot) { } if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceTerminal) { autonomousStep += 1; - leftpower = 0; - rightpower = 0; + startDistance = robot.getDriveDistanceInchesLeft(); + startTime = System.currentTimeMillis(); } break; - case 8: + case 8: //collect ball at Terminal leftpower = 0; rightpower = 0; - startDistance = robot.getDriveDistanceInchesLeft(); autonomousStep += 1; + robot.setPipeline(0); case 9: // drive back correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); - leftpower = -defaultPower + correction; - rightpower = -defaultPower - correction; + leftpower = -1*(defaultPower + correction); + rightpower = -1*(defaultPower - correction); if(Math.abs(robot.getDriveDistanceInchesLeft() - startDistance) >= distanceTerminal - rampDownDist){ double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist, @@ -177,7 +220,7 @@ public void autonomousPeriodic(GenericRobot robot) { } break; case 11: //turn shooter off - if (System.currentTimeMillis()-startTime >= 250){ + if (System.currentTimeMillis()-startTime >= 1000){ robot.setActivelyShooting(false); autonomousStep += 1; } @@ -189,6 +232,5 @@ public void autonomousPeriodic(GenericRobot robot) { } robot.drivePercent(leftpower, rightpower); tracker.turretMove(robot); - } } diff --git a/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java index 66007e5..3de91cf 100644 --- a/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java +++ b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java @@ -210,7 +210,7 @@ public void autonomousPeriodic(GenericRobot robot) { autonomousStep += 1; robot.setPipeline(0); break; - case 9: //Drive forward a better shooting position + case 9: //Drive back to Ball C correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); leftpower = -1*(defaultPower - correction); @@ -247,7 +247,5 @@ public void autonomousPeriodic(GenericRobot robot) { break; } robot.drivePercent(leftpower, rightpower); - - } } From 591bc914020a629d199d64e1250fde3d5716defc Mon Sep 17 00:00:00 2001 From: Louminator Date: Tue, 1 Mar 2022 21:35:16 -0500 Subject: [PATCH 64/72] Added comment. --- src/main/java/frc/robot/generic/Lightning.java | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/main/java/frc/robot/generic/Lightning.java b/src/main/java/frc/robot/generic/Lightning.java index b1b0695..ea41273 100644 --- a/src/main/java/frc/robot/generic/Lightning.java +++ b/src/main/java/frc/robot/generic/Lightning.java @@ -325,6 +325,14 @@ 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(); From 987e25916277cf34147e1d88be9b3e76f5edc15a Mon Sep 17 00:00:00 2001 From: JW314 Date: Wed, 2 Mar 2022 18:38:26 -0500 Subject: [PATCH 65/72] Did an oops: get changed to set --- .idea/jarRepositories.xml | 5 +++++ .idea/misc.xml | 2 +- src/main/java/frc/robot/generic/Lightning.java | 8 ++++---- 3 files changed, 10 insertions(+), 5 deletions(-) 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..d7070eb 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -4,5 +4,5 @@ - + \ No newline at end of file diff --git a/src/main/java/frc/robot/generic/Lightning.java b/src/main/java/frc/robot/generic/Lightning.java index 84a7618..d05884a 100644 --- a/src/main/java/frc/robot/generic/Lightning.java +++ b/src/main/java/frc/robot/generic/Lightning.java @@ -102,15 +102,15 @@ public Lightning(){ shooterAPIDController.setI(0); shooterAPIDController.setD(2.5e-1); shooterAPIDController.setFF(1.67e-4); - shooterAPIDController.getIZone(500); - shooterAPIDController.getDFilter(0); + shooterAPIDController.setIZone(500); + shooterAPIDController.setDFilter(0); shooterBPIDController.setP(2.5e-4); shooterBPIDController.setI(0); shooterBPIDController.setD(2.5e-1); shooterBPIDController.setFF(1.67e-4); - shooterBPIDController.getIZone(500); - shooterBPIDController.getDFilter(0); + shooterBPIDController.setIZone(500); + shooterBPIDController.setDFilter(0); shootReadyTimer = System.currentTimeMillis(); From 8a4270d09971af4abec52fc12246ba194c13f66b Mon Sep 17 00:00:00 2001 From: Malti Date: Wed, 2 Mar 2022 21:36:00 -0500 Subject: [PATCH 66/72] Almost hang!! Shooter presets ready! TODO: remove all delays and speed up climb --- src/main/java/frc/robot/Robot.java | 31 ++++++++++++++----- src/main/java/frc/robot/command/Hang.java | 17 +++++++--- .../java/frc/robot/generic/Lightning.java | 23 +++++++++----- 3 files changed, 51 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e412b96..75ea8a5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -51,6 +51,8 @@ public class Robot extends TimedRobot { double turretPower; boolean hang = false; int countHang = 0; + int countShoot = 0; + boolean shoot = false; boolean reset = true; double maxCurrentLeftA = 0; double maxCurrentLeftB = 0; @@ -71,7 +73,6 @@ public class Robot extends TimedRobot { double defaultClimbPower = 0.2; double shooterTargetRPM = 0; - double pitchChange = 0; double defCollectorPower = 1; double defIndexerPower = 1; @@ -228,10 +229,13 @@ public void autonomousPeriodic() { @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); } @Override @@ -245,16 +249,16 @@ public void teleopPeriodic() { turretPitch = 0.38; break; case EAST: - targetRPM = 4000; - turretPitch = 0.00; + targetRPM = 2000; + turretPitch = 0.8; break; case SOUTH: ////CLOSE SHOT - targetRPM = 3500; - turretPitch = 0.12; + targetRPM = 3400; + turretPitch = 0.00; break; case WEST: - targetRPM = 5500; - turretPitch = 0.00; + targetRPM = 3700; + turretPitch = 0.1; break; } @@ -270,6 +274,7 @@ public void teleopPeriodic() { if (!hang) { reset = true; + robot.turnOffPTO(); //////////////////////////////////////////////////DRIVETRAIN CONTROL @@ -364,7 +369,16 @@ else if(joystick.getRawButton(4)){ //////////////////////////////////////////////////////////SHOOTER CODE BEGINS - if (xbox.getRawButton(3)) { + if (xbox.getRawButtonPressed(3)) { + countShoot = (countShoot + 1) % 2; + } + if (countShoot == 0){ + shoot = false; + } + else{ + shoot = true; + } + if (shoot){ shooterTargetRPM = targetRPM; } else { shooterTargetRPM = 0; @@ -505,6 +519,7 @@ public void disabledInit() { @Override public void disabledPeriodic() { + xbox.getRawButtonPressed(3); joystick.getRawButtonPressed(8); hang = false; countHang = 0; diff --git a/src/main/java/frc/robot/command/Hang.java b/src/main/java/frc/robot/command/Hang.java index d74b452..f42e7e8 100644 --- a/src/main/java/frc/robot/command/Hang.java +++ b/src/main/java/frc/robot/command/Hang.java @@ -50,6 +50,9 @@ public class Hang extends GenericCommand{ double startHeightLeft = 0; double startHeightRight = 0; double turretPower = 0; + double level = 7; + double leveltol = 2; + PIDController turretPIDController; @@ -82,7 +85,7 @@ public void step(GenericRobot robot){ if (tapeAlign) { - /*if (commandStep > -1) { + if (commandStep > -1) { if ((robot.getAlternateTurretAngle() <48) && (robot.getAlternateTurretAngle() > 42)){ turretPower = 0; @@ -92,7 +95,7 @@ public void step(GenericRobot robot){ turretPower = -turretPIDController.calculate(robot.getAlternateTurretAngle() - 45); } } - robot.setTurretPowerPct(turretPower);*/ + robot.setTurretPowerPct(turretPower); robot.raiseCollector(); switch (commandStep) { /////////////tapeAlign Code case -1: @@ -256,7 +259,7 @@ else if (!robot.getFloorSensorLeft()) { break; case 3: ///////////unlock rotation piston to send arms back robot.setArmsBackward(); - if (System.currentTimeMillis() - startingTime >= 1000) { + if (System.currentTimeMillis() - startingTime >= 3000) { commandStep = 11; } break; @@ -405,7 +408,7 @@ else if (!robot.getFloorSensorLeft()) { } case 16://///////once in contact move arms back again with the piston and swiiiiing robot.setArmsBackward(); - if (System.currentTimeMillis() - startingTime >= 1000) { + if (System.currentTimeMillis() - startingTime >= 3000) { commandStep += 1;//TODO:change back } break; @@ -470,6 +473,12 @@ else if (!robot.getFloorSensorLeft()) { } + 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/Lightning.java b/src/main/java/frc/robot/generic/Lightning.java index ea41273..dd3a4da 100644 --- a/src/main/java/frc/robot/generic/Lightning.java +++ b/src/main/java/frc/robot/generic/Lightning.java @@ -34,7 +34,7 @@ public class Lightning implements GenericRobot { //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(1); + Servo elevationRight = new Servo(8); RelativeEncoder encoderRightA = rightMotorA.getEncoder(); @@ -94,9 +94,11 @@ public Lightning(){ 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); @@ -107,12 +109,12 @@ public Lightning(){ shooterAPIDController.getIZone(500); shooterAPIDController.getDFilter(0); - shooterBPIDController.setP(5.0e-4); + /*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); + shooterBPIDController.getDFilter(0);*/ shootReadyTimer = System.currentTimeMillis(); @@ -223,12 +225,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 @@ -337,7 +339,7 @@ public double encoderTurretTicksPerDegree(){ public double getAlternateTurretAngle(){ double raw = encoderTurretAlt.getPosition(); double out; - double offset = 185-45; + double offset = 185-45+30; out = (raw * 136.467) - 5.73 - offset; if (out>360) { @@ -436,12 +438,17 @@ public double getShooterTargetHeight() { @Override public void setShooterRPM(double topRPM, double bottomRPM) { 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 From 8c2e3296c13bf61c14ee3df82814a7e1bfd7bde0 Mon Sep 17 00:00:00 2001 From: Louminator Date: Thu, 3 Mar 2022 10:58:24 -0500 Subject: [PATCH 67/72] Update Robot.java When you wake up, say something. --- src/main/java/frc/robot/Robot.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 75ea8a5..80c7dd2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -88,6 +88,7 @@ public class Robot extends TimedRobot { @Override public void robotInit() { + System.out.println("Klaatu barada nikto"); robot.setTurretPitchPosition(0); } From c1ebbafde22d405f060075d660cf13fecdff2cfc Mon Sep 17 00:00:00 2001 From: Malti Date: Thu, 3 Mar 2022 20:19:35 -0500 Subject: [PATCH 68/72] Changes from auto-testing. Confirmed: autoArc works --- src/main/java/frc/robot/Robot.java | 11 +-- .../java/frc/robot/autonomous/autoArc.java | 77 ++++++++++++------- src/main/java/frc/robot/command/Hang.java | 25 +++--- .../java/frc/robot/generic/GenericRobot.java | 3 + .../java/frc/robot/generic/Lightning.java | 5 ++ 5 files changed, 75 insertions(+), 46 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 80c7dd2..11e4d85 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -83,6 +83,7 @@ public class Robot extends TimedRobot { double turretPitch = 0; + PIDController turretPIDController; @@ -120,8 +121,8 @@ public void robotPeriodic() { SmartDashboard.putNumber("RightACurrentMax", maxCurrentRightA); SmartDashboard.putNumber("RightBCurrentMax", maxCurrentRightB); - SmartDashboard.putNumber("LimelightX", robot.getTargetX()); - SmartDashboard.putNumber("LimelightY", robot.getTargetY()); + //SmartDashboard.putNumber("LimelightX", robot.getTargetX()); + //SmartDashboard.putNumber("LimelightY", robot.getTargetY()); SmartDashboard.putNumber("LimelightArea", robot.getTargetArea()); SmartDashboard.putNumber("Drive left pct", robot.getDriveLeftPercentage()); @@ -172,7 +173,7 @@ public void robotPeriodic() { 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 position", robot.getTurretPitchPosition()); SmartDashboard.putNumber("Turret pitch motor pct", robot.getTurretPitchPowerPct()); @@ -253,12 +254,12 @@ public void teleopPeriodic() { targetRPM = 2000; turretPitch = 0.8; break; - case SOUTH: ////CLOSE SHOT + case SOUTH: ////CLOSE SHOT--> collector out targetRPM = 3400; turretPitch = 0.00; break; case WEST: - targetRPM = 3700; + targetRPM = 3700; //////////collector facing turretPitch = 0.1; break; } diff --git a/src/main/java/frc/robot/autonomous/autoArc.java b/src/main/java/frc/robot/autonomous/autoArc.java index cbb12a6..8015d6a 100644 --- a/src/main/java/frc/robot/autonomous/autoArc.java +++ b/src/main/java/frc/robot/autonomous/autoArc.java @@ -32,11 +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) { @@ -60,13 +59,55 @@ public void autonomousInit(GenericRobot robot) { autonomousStep = 0; time = false; counter = 0; + collect = false; + robot.setPipeline(0); } @Override public void autonomousPeriodic(GenericRobot robot) { + if(robot.isTargetFound()) { + averageX[counter % averageTurretXSize] = robot.getTargetX(); + counter++; + } + + double average = 0; + for(double i: averageX){ + average += i; + } + average /= averageTurretXSize; + + double currentTurretPower = 0; + + if(robot.isTargetFound()){ + currentTurretPower = turretPIDController.calculate(average); + }else{ + turretPIDController.reset(); + } + + + 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){ - robot.getCargo(); + if (autonomousStep <= 4){ + robot.getCargo(); + } + else if (!collect){ + robot.setCollectorIntakePercentage(-1); + } + else{ + robot.getCargo(); + } robot.shoot(); robot.setTurretPitchPosition(.38); } @@ -92,7 +133,7 @@ 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; } @@ -111,8 +152,7 @@ public void autonomousPeriodic(GenericRobot robot) { break; case 3: if (System.currentTimeMillis() - startingTime >= 1000){ - robot.setActivelyShooting(true); - robot.setShooterTargetRPM(500); //super low power to yeet out undesirable cargo + robot.setActivelyShooting(false); autonomousStep += 1; } break; @@ -126,7 +166,7 @@ public void autonomousPeriodic(GenericRobot robot) { breakRobot = true; } - if (Math.abs(Math.abs(currentYaw - startYaw)-pivotDeg) <= 1.5){ + if (Math.abs(Math.abs(currentYaw - startYaw)-pivotDeg) <= 2){ if (!time){ startingTime = System.currentTimeMillis(); time = true; @@ -167,8 +207,7 @@ public void autonomousPeriodic(GenericRobot robot) { autonomousStep += 1; } if (currentDistInches - startInches >= outerArcDist - 24){ - robot.setShooterTargetRPM(5000); //set it back to where we want - robot.setActivelyShooting(false); + collect = true; } break; @@ -187,27 +226,7 @@ public void autonomousPeriodic(GenericRobot robot) { } robot.drivePercent(leftPower, rightPower); - if(robot.isTargetFound()) { - averageX[counter % averageTurretXSize] = robot.getTargetX(); - counter++; - } - average = 0; - for(double i: averageX){ - average += i; - } - average /= averageTurretXSize; - - currentTurretPower = 0; - if(robot.isTargetFound()){ - currentTurretPower = turretPIDController.calculate(average); - }else{ - turretPIDController.reset(); - } - if((!robot.isTargetFound()) && (System.currentTimeMillis() - startingTime < 2000)) { - currentTurretPower = .2; - } - robot.setTurretPowerPct(currentTurretPower); } diff --git a/src/main/java/frc/robot/command/Hang.java b/src/main/java/frc/robot/command/Hang.java index f42e7e8..debaa0e 100644 --- a/src/main/java/frc/robot/command/Hang.java +++ b/src/main/java/frc/robot/command/Hang.java @@ -43,8 +43,8 @@ public class Hang extends GenericCommand{ int countRight = 0; double leftArmPower = 0; double rightArmPower = 0; - double defaultClimbPowerUp = .5; - double defaultClimbPowerDown = -.5; + double defaultClimbPowerUp = .75; + double defaultClimbPowerDown = -.75; boolean leftArrived = false; boolean rightArrived = false; double startHeightLeft = 0; @@ -209,14 +209,15 @@ else if (!robot.getFloorSensorLeft()) { 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 + /*case 1: ///////////unlock rotation piston to send arms forward robot.setArmsForward(); //TODO: skip step commandStep += 1; - break; + break;*/ case 2: //////raise climber arms (skip 10 steps after in case we need to scoot/scoot if (!robot.getClimbSensorLeft() && countLeft == 0){ @@ -259,7 +260,7 @@ else if (!robot.getFloorSensorLeft()) { break; case 3: ///////////unlock rotation piston to send arms back robot.setArmsBackward(); - if (System.currentTimeMillis() - startingTime >= 3000) { + if (System.currentTimeMillis() - startingTime >= 1000) { commandStep = 11; } break; @@ -315,7 +316,7 @@ else if (!robot.getFloorSensorLeft()) { leftArmPower = 0; rightArmPower = 0; //TODO: change - commandStep = 30; + commandStep += 1; leftArrived = false; rightArrived = false; startHeightLeft = robot.armHeightLeft(); @@ -324,11 +325,7 @@ else if (!robot.getFloorSensorLeft()) { } break; - case 30: //delay :)TODO:change - if (System.currentTimeMillis() - startingTime >= 5000){ - commandStep = 12; - } - break; + case 12: /////////////raise arms slightly if (Math.abs(robot.armHeightLeft()-startHeightLeft) >= escapeHeight){ leftArmPower = 0; @@ -408,7 +405,7 @@ else if (!robot.getFloorSensorLeft()) { } case 16://///////once in contact move arms back again with the piston and swiiiiing robot.setArmsBackward(); - if (System.currentTimeMillis() - startingTime >= 3000) { + if (System.currentTimeMillis() - startingTime >= 1000) { commandStep += 1;//TODO:change back } break; @@ -479,6 +476,10 @@ else if (!robot.getFloorSensorLeft()) { 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/generic/GenericRobot.java b/src/main/java/frc/robot/generic/GenericRobot.java index 44c67ba..6d11af5 100644 --- a/src/main/java/frc/robot/generic/GenericRobot.java +++ b/src/main/java/frc/robot/generic/GenericRobot.java @@ -430,6 +430,9 @@ public default boolean inTheRightPlace(){ //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 dd3a4da..a6ad0bb 100644 --- a/src/main/java/frc/robot/generic/Lightning.java +++ b/src/main/java/frc/robot/generic/Lightning.java @@ -684,4 +684,9 @@ public boolean isReadyToShoot(){ public void setActivelyShooting(boolean isShooting){ isActivelyShooting = isShooting; } + + @Override + public double getDriveCurrent(){ + return leftMotorA.getOutputCurrent(); + } } From 9d28ccc8ebb8b55fe29621fd6ce0d13a860fa3a6 Mon Sep 17 00:00:00 2001 From: Malti Date: Thu, 3 Mar 2022 21:29:00 -0500 Subject: [PATCH 69/72] Fixed hang bug. Did full climb!!!! --- src/main/java/frc/robot/Robot.java | 33 +++++++++++++++++------ src/main/java/frc/robot/command/Hang.java | 6 +++++ 2 files changed, 31 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 11e4d85..8bcedce 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -82,6 +82,8 @@ public class Robot extends TimedRobot { double targetRPM = 0; double turretPitch = 0; + boolean turnTo45 = false; + boolean turnTo225 = false; PIDController turretPIDController; @@ -238,6 +240,8 @@ public void teleopInit() { hang = false; countHang = 0; xbox.getRawButtonPressed(3); + turnTo45 = false; + turnTo225 = false; } @Override @@ -257,10 +261,14 @@ public void teleopPeriodic() { case SOUTH: ////CLOSE SHOT--> collector out targetRPM = 3400; turretPitch = 0.00; + turnTo225 = true; + turnTo45 = false; break; case WEST: targetRPM = 3700; //////////collector facing turretPitch = 0.1; + turnTo45 = true; + turnTo225 = false; break; } @@ -330,24 +338,33 @@ else if(joystick.getRawButton(4)){ average += averageX[i]; average /= averageTurretXSize; - if ((xbox.getRawAxis(2) > 0.10) & robot.isTargetFound()) { ////////////AUTO-AIM + if ((xbox.getRawAxis(2) > 0.10) && robot.isTargetFound()) { ////////////AUTO-AIM turretPower = turretPIDController.calculate(average); - } else { + turnTo45 = false; + turnTo225 = false; + } + else if(turnTo45) { + turretPower = -turretPIDController.calculate(robot.getAlternateTurretAngle()-45); + } + else if (turnTo225){ + turretPower = -turretPIDController.calculate(robot.getAlternateTurretAngle()-225); + } + 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 { turretPower = 0.0; } } - if (joystick.getRawButtonPressed(5)){ - turretPIDController.reset(); - } - if (joystick.getRawButton(5)){ - turretPower = -turretPIDController.calculate(robot.getAlternateTurretAngle()-45); - } + + /////////////////////////////////////////////////////TURRET CONTROL ENDS diff --git a/src/main/java/frc/robot/command/Hang.java b/src/main/java/frc/robot/command/Hang.java index debaa0e..cf29de9 100644 --- a/src/main/java/frc/robot/command/Hang.java +++ b/src/main/java/frc/robot/command/Hang.java @@ -61,6 +61,10 @@ public void begin(GenericRobot robot){ 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()); @@ -206,6 +210,8 @@ else if (!robot.getFloorSensorLeft()) { //reset encoders robot.turnOnPTO(); robot.resetEncoders(); + leftArmPower = 0; + rightArmPower = 0; countLeft = 0; countRight = 0; if (System.currentTimeMillis() - startingTime >= 5000){ From 358fd4971ca156ea8789fd29cce8fdd5bdd960d9 Mon Sep 17 00:00:00 2001 From: Malti Date: Fri, 4 Mar 2022 23:28:57 -0500 Subject: [PATCH 70/72] Changes from Friday. --- src/main/java/frc/robot/Robot.java | 64 +++++++++++---- .../autonomous/BallAtoTerminalReturn.java | 21 ++--- .../frc/robot/command/HangWithoutAlign.java | 82 +++++++++++-------- .../java/frc/robot/generic/Lightning.java | 18 ++-- 4 files changed, 119 insertions(+), 66 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8bcedce..9246284 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -11,10 +11,8 @@ import frc.robot.autonomous.*; import frc.robot.autonomous.GenericAutonomous; import frc.robot.command.*; -import frc.robot.generic.Falcon; import frc.robot.generic.GenericRobot; import frc.robot.generic.Lightning; -import frc.robot.generic.TurretBot; import java.util.Arrays; import java.util.Map; @@ -27,7 +25,7 @@ public class Robot extends TimedRobot { //Add new Autos here when they're authored public static final GenericAutonomous autoArc = new autoArc(), - simpleATerminal = new BallAtoTerminal(), + ATerminalReturn = new BallAtoTerminalReturn(), simpleBTerminal = new BallBtoTerminal(), simpleCTerminal = new BallCtoTerminal(), CTerminalReturn = new BallCtoTerminalReturn(), @@ -40,6 +38,7 @@ public class Robot extends TimedRobot { GenericCommand command = new Hang(); Joystick xbox = new Joystick(1); GenericAutonomous autonomous = CTerminalReturn; + GenericCommand testHang = new HangWithoutAlign(); int averageTurretXSize = 2; @@ -85,6 +84,8 @@ public class Robot extends TimedRobot { boolean turnTo45 = false; boolean turnTo225 = false; + boolean isShooting = false; + PIDController turretPIDController; @@ -98,6 +99,13 @@ public void robotInit() { @Override public void robotPeriodic() { + if (countShoot == 0){ + isShooting = false; + } + else{ + isShooting = robot.isReadyToShoot(); + } + if (robot.getLeftACurrent() > maxCurrentLeftA) { maxCurrentLeftA = robot.getLeftACurrent(); } @@ -218,6 +226,8 @@ public void robotPeriodic() { SmartDashboard.putNumber("CommandStep", command.commandStep); + SmartDashboard.putBoolean("Robot is Shooting?", isShooting); + } @@ -265,7 +275,7 @@ public void teleopPeriodic() { turnTo45 = false; break; case WEST: - targetRPM = 3700; //////////collector facing + targetRPM = 3600; //////////collector facing turretPitch = 0.1; turnTo45 = true; turnTo225 = false; @@ -339,16 +349,12 @@ else if(joystick.getRawButton(4)){ average /= averageTurretXSize; if ((xbox.getRawAxis(2) > 0.10) && robot.isTargetFound()) { ////////////AUTO-AIM - turretPower = turretPIDController.calculate(average); turnTo45 = false; turnTo225 = false; + turretPower = turretPIDController.calculate(average); + } - else if(turnTo45) { - turretPower = -turretPIDController.calculate(robot.getAlternateTurretAngle()-45); - } - else if (turnTo225){ - turretPower = -turretPIDController.calculate(robot.getAlternateTurretAngle()-225); - } + else { turretPIDController.reset(); if (xbox.getRawButton(6)) { @@ -359,7 +365,14 @@ else if (turnTo225){ turretPower = 0.45; turnTo45 = false; turnTo225 = false; - } else { + } + else if(turnTo45) { + turretPower = -turretPIDController.calculate(robot.getAlternateTurretAngle()-45); + } + else if (turnTo225){ + turretPower = -turretPIDController.calculate(robot.getAlternateTurretAngle()-225); + } + else { turretPower = 0.0; } } @@ -548,11 +561,9 @@ public void disabledPeriodic() { } if (joystick.getRawButton(4)) autonomous = autoArc; - if (joystick.getRawButton(5)) autonomous = simpleATerminal; - if (joystick.getRawButton(6)) autonomous = simpleBTerminal; - if (joystick.getRawButton(7)) autonomous = simpleCTerminal; + if (joystick.getRawButton(5)) autonomous = ATerminalReturn; + if (joystick.getRawButton(6)) autonomous = BTerminalReturn; if (joystick.getRawButton(9)) autonomous = CTerminalReturn; - if (joystick.getRawButton(11)) autonomous = simpleB; } @@ -660,6 +671,27 @@ public void testPeriodic() { 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; + } + } diff --git a/src/main/java/frc/robot/autonomous/BallAtoTerminalReturn.java b/src/main/java/frc/robot/autonomous/BallAtoTerminalReturn.java index d9082ab..064be54 100644 --- a/src/main/java/frc/robot/autonomous/BallAtoTerminalReturn.java +++ b/src/main/java/frc/robot/autonomous/BallAtoTerminalReturn.java @@ -23,9 +23,6 @@ public class BallAtoTerminalReturn extends GenericAutonomous { double[] averageTurretX = new double [averageTurretXSize]; int counter = 0; - double indexerPct; - double collectorPct; - double distanceA = 44.2; double distanceTerminal = 259.26; double angleA = 88.74; @@ -33,8 +30,7 @@ public class BallAtoTerminalReturn extends GenericAutonomous { PIDController PIDDriveStraight; PIDController PIDPivot; - TurretTracker tracker = new TurretTracker(); - + PIDController turretPIDController; @Override public void autonomousInit(GenericRobot robot) { autonomousStep = 0; @@ -44,7 +40,7 @@ public void autonomousInit(GenericRobot robot) { readyToShoot = false; PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDpivotD()); PIDPivot = new PIDController(robot.getPIDpivotP(), robot.getPIDpivotI(), robot.getPIDpivotD()); - tracker.turretInit(robot); + turretPIDController = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD()); robot.setPipeline(0); } @@ -62,17 +58,19 @@ public void autonomousPeriodic(GenericRobot robot) { } average /= averageTurretXSize; + currentTurretPower = turretPIDController.calculate(average); + if (autonomousStep < 4){ if((!robot.isTargetFound()) && (System.currentTimeMillis() - startTime < 5000)) { currentTurretPower = .3; } } - if ((autonomousStep>=4) && (autonomousStep < 8)){ + if ((autonomousStep>=4) && (autonomousStep < 10)){ if((!robot.isTargetFound()) && (System.currentTimeMillis() - startTime < 5000)) { currentTurretPower = -.2; } } - robot.setTurretPowerPct(currentTurretPower); + if (autonomousStep >= 1){ robot.getCargo(); @@ -82,10 +80,10 @@ public void autonomousPeriodic(GenericRobot robot) { robot.setTurretPitchPosition(.38); } else{ robot.setCollectorIntakePercentage(0); - robot.setTurretPowerPct(0); + currentTurretPower = 0; } - tracker.turretUpdate(robot); + robot.setTurretPowerPct(currentTurretPower); switch(autonomousStep){ case 0: //reset @@ -103,7 +101,7 @@ public void autonomousPeriodic(GenericRobot robot) { } break; case 1: //drive to ball A - collectorPct = 1; + correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw); leftpower = defaultPower + correction; @@ -231,6 +229,5 @@ public void autonomousPeriodic(GenericRobot robot) { break; } robot.drivePercent(leftpower, rightpower); - tracker.turretMove(robot); } } diff --git a/src/main/java/frc/robot/command/HangWithoutAlign.java b/src/main/java/frc/robot/command/HangWithoutAlign.java index d152829..9592f06 100644 --- a/src/main/java/frc/robot/command/HangWithoutAlign.java +++ b/src/main/java/frc/robot/command/HangWithoutAlign.java @@ -16,7 +16,7 @@ public class HangWithoutAlign extends GenericCommand{ double startDistance; double differenceDistance; - double sensorDist = 12.0; + double sensorDist = 21.0; double Tapetheta = 0; double correction; @@ -29,11 +29,11 @@ public class HangWithoutAlign extends GenericCommand{ double lTraveled; - double fwd = 71.6; + double fwd = 49.5; PIDController PIDSteering; boolean tapeAlign; - /////^^^^^^^^^^^Stuff for tapeAlign + /////^^^^^^^^^^^Stuff for tapeAlign/ //////////////Now the real stuff @@ -43,12 +43,17 @@ public class HangWithoutAlign extends GenericCommand{ int countRight = 0; double leftArmPower = 0; double rightArmPower = 0; - double defaultClimbPowerUp = -.5; - double defaultClimbPowerDown = .5; + 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){ @@ -56,31 +61,26 @@ public void begin(GenericRobot robot){ commandStep = -1; leftSensor = false; rightSensor = false; + leftArmPower = 0; + rightArmPower = 0; + leftPower = 0; + rightPower = 0; lTraveled = 0; - fwd = 71.6; + 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); - //////////////////////////start the real stuff now -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - switch (commandStep){ + switch (commandStep){ case -1: commandStep += 1; break; @@ -88,17 +88,20 @@ public void step(GenericRobot robot){ //reset encoders robot.turnOnPTO(); robot.resetEncoders(); + leftArmPower = 0; + rightArmPower = 0; countLeft = 0; countRight = 0; - if (System.currentTimeMillis() - startingTime >= 50){ - commandStep += 1; + 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(); + /*case 1: ///////////unlock rotation piston to send arms forward + robot.setArmsForward(); //TODO: skip step commandStep += 1; - break; + break;*/ case 2: //////raise climber arms (skip 10 steps after in case we need to scoot/scoot if (!robot.getClimbSensorLeft() && countLeft == 0){ @@ -133,14 +136,17 @@ public void step(GenericRobot robot){ countRight = 0; leftArmPower = 0; rightArmPower = 0; + startingTime = System.currentTimeMillis(); commandStep += 1; } break; - case 3: ///////////unlock rotation piston to send arms forward + case 3: ///////////unlock rotation piston to send arms back robot.setArmsBackward(); - commandStep = 11; + if (System.currentTimeMillis() - startingTime >= 1000) { + commandStep = 11; + } break; /*case 2: //////disable PTO robot.turnOffPTO(); @@ -193,7 +199,8 @@ public void step(GenericRobot robot){ countLeft = 0; leftArmPower = 0; rightArmPower = 0; - commandStep = 30; + //TODO: change + commandStep += 1; leftArrived = false; rightArrived = false; startHeightLeft = robot.armHeightLeft(); @@ -202,11 +209,7 @@ public void step(GenericRobot robot){ } break; - case 30: //delay :)TODO:change - if (System.currentTimeMillis() - startingTime >= 5000){ - commandStep = 12; - } - break; + case 12: /////////////raise arms slightly if (Math.abs(robot.armHeightLeft()-startHeightLeft) >= escapeHeight){ leftArmPower = 0; @@ -267,6 +270,7 @@ public void step(GenericRobot robot){ rightArmPower = 0; leftArmPower = 0; commandStep = 16; //////////skip over step 15 + startingTime = System.currentTimeMillis(); } break; case 15:///change to check with pitch and roll @@ -285,7 +289,9 @@ public void step(GenericRobot robot){ } case 16://///////once in contact move arms back again with the piston and swiiiiing robot.setArmsBackward(); - commandStep += 1; + 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){ @@ -340,13 +346,23 @@ public void step(GenericRobot robot){ 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/generic/Lightning.java b/src/main/java/frc/robot/generic/Lightning.java index a6ad0bb..dff4606 100644 --- a/src/main/java/frc/robot/generic/Lightning.java +++ b/src/main/java/frc/robot/generic/Lightning.java @@ -77,6 +77,8 @@ public class Lightning implements GenericRobot { //shootReadyTimer is used to check if shooter ready long shootReadyTimer; + double attemptedRPM = 0; + public Lightning(){ boolean invertLeft = false; boolean invertRight = true; @@ -266,7 +268,7 @@ public void getCargo(){ public void shoot(){ double shooterRPM = defaultShooterTargetRPM; setShooterRPM(shooterRPM, shooterRPM); - if (getShooterRPMTop() >= (shooterRPM-300) && getShooterRPMBottom() >= (shooterRPM-300)){ + if (getShooterRPMTop() >= (shooterRPM-100) && getShooterRPMBottom() >= (shooterRPM-100)){ canShoot = true; } else{ @@ -339,7 +341,7 @@ public double encoderTurretTicksPerDegree(){ public double getAlternateTurretAngle(){ double raw = encoderTurretAlt.getPosition(); double out; - double offset = 185-45+30; + double offset = 185-45+30+5+163+15; out = (raw * 136.467) - 5.73 - offset; if (out>360) { @@ -437,6 +439,7 @@ public double getShooterTargetHeight() { @Override public void setShooterRPM(double topRPM, double bottomRPM) { + attemptedRPM = topRPM; setShooterRPMTop(topRPM); //setShooterRPMBottom(bottomRPM); } @@ -646,7 +649,7 @@ public void resetAttitude() { @Override public double turretPIDgetP() { - return 3.0e-2; + return 2.0e-2; } @Override @@ -656,7 +659,7 @@ public double turretPIDgetI() { @Override public double turretPIDgetD() { - return 3.0e-4; + return 1.0e-3; } @@ -677,7 +680,12 @@ public boolean isActivelyShooting(){ //TODO: Add check using isReadyToShoot() function? @Override public boolean isReadyToShoot(){ - return true; + if (Math.abs(getShooterRPMTop() -attemptedRPM) <= 300){ + return true; + } + else{ + return false; + } } @Override From 5865b10d9cdb24d5d9f3bdaa1bc692095797514e Mon Sep 17 00:00:00 2001 From: Malti Date: Sat, 5 Mar 2022 08:06:15 -0500 Subject: [PATCH 71/72] SimpleA ready to test --- .../java/frc/robot/autonomous/BallAtoTerminalReturn.java | 2 +- src/main/java/frc/robot/generic/Lightning.java | 9 ++------- 2 files changed, 3 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/autonomous/BallAtoTerminalReturn.java b/src/main/java/frc/robot/autonomous/BallAtoTerminalReturn.java index 064be54..878ce3e 100644 --- a/src/main/java/frc/robot/autonomous/BallAtoTerminalReturn.java +++ b/src/main/java/frc/robot/autonomous/BallAtoTerminalReturn.java @@ -67,7 +67,7 @@ public void autonomousPeriodic(GenericRobot robot) { } if ((autonomousStep>=4) && (autonomousStep < 10)){ if((!robot.isTargetFound()) && (System.currentTimeMillis() - startTime < 5000)) { - currentTurretPower = -.2; + currentTurretPower = .2; } } diff --git a/src/main/java/frc/robot/generic/Lightning.java b/src/main/java/frc/robot/generic/Lightning.java index dff4606..57ba8ca 100644 --- a/src/main/java/frc/robot/generic/Lightning.java +++ b/src/main/java/frc/robot/generic/Lightning.java @@ -268,7 +268,7 @@ public void getCargo(){ public void shoot(){ double shooterRPM = defaultShooterTargetRPM; setShooterRPM(shooterRPM, shooterRPM); - if (getShooterRPMTop() >= (shooterRPM-100) && getShooterRPMBottom() >= (shooterRPM-100)){ + if (getShooterRPMTop() >= (shooterRPM-300) && getShooterRPMBottom() >= (shooterRPM-300)){ canShoot = true; } else{ @@ -680,12 +680,7 @@ public boolean isActivelyShooting(){ //TODO: Add check using isReadyToShoot() function? @Override public boolean isReadyToShoot(){ - if (Math.abs(getShooterRPMTop() -attemptedRPM) <= 300){ - return true; - } - else{ - return false; - } + return true; } @Override From 61a7428c5d58511a70a8381351133c6d29f8fd5c Mon Sep 17 00:00:00 2001 From: Malti Date: Sat, 5 Mar 2022 19:06:08 -0500 Subject: [PATCH 72/72] Changes from competition! --- src/main/java/frc/robot/Robot.java | 5 +- .../autonomous/BallAtoTerminalReturn.java | 1 + .../autonomous/BallBtoTerminalReturn.java | 3 +- .../autonomous/BallCtoTerminalReturn.java | 10 +- .../java/frc/robot/autonomous/ShortRun.java | 164 ++++++++++++++++++ src/main/java/frc/robot/command/Hang.java | 7 +- .../frc/robot/command/HangWithoutAlign.java | 5 +- .../java/frc/robot/generic/Lightning.java | 2 +- 8 files changed, 184 insertions(+), 13 deletions(-) create mode 100644 src/main/java/frc/robot/autonomous/ShortRun.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9246284..336d644 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -31,7 +31,8 @@ public class Robot extends TimedRobot { CTerminalReturn = new BallCtoTerminalReturn(), BTerminalReturn = new BallBtoTerminalReturn(), simpleB = new BallSimpleB(), - calibration = new Calibration(); + calibration = new Calibration(), + shortRun = new ShortRun(); GenericRobot robot = new Lightning(); Joystick joystick = new Joystick(0); @@ -564,11 +565,13 @@ public void disabledPeriodic() { if (joystick.getRawButton(5)) autonomous = ATerminalReturn; if (joystick.getRawButton(6)) autonomous = BTerminalReturn; if (joystick.getRawButton(9)) autonomous = CTerminalReturn; + if (joystick.getRawButton(7)) autonomous = shortRun; } @Override public void testInit() { + joystick.getRawButtonPressed(4); } @Override diff --git a/src/main/java/frc/robot/autonomous/BallAtoTerminalReturn.java b/src/main/java/frc/robot/autonomous/BallAtoTerminalReturn.java index 878ce3e..589e60e 100644 --- a/src/main/java/frc/robot/autonomous/BallAtoTerminalReturn.java +++ b/src/main/java/frc/robot/autonomous/BallAtoTerminalReturn.java @@ -75,6 +75,7 @@ public void autonomousPeriodic(GenericRobot robot) { if (autonomousStep >= 1){ robot.getCargo(); robot.shoot(); + robot.setShooterTargetRPM(3700); } if (autonomousStep >= 1 && autonomousStep <=10){ robot.setTurretPitchPosition(.38); diff --git a/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java b/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java index 0c5733b..edf92fa 100644 --- a/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java +++ b/src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java @@ -17,7 +17,7 @@ public class BallBtoTerminalReturn extends GenericAutonomous { double correction; double distanceB = 61.5; - double distanceTerminal = 154.6; + double distanceTerminal = 142.6; double rampDownDist = 10; PIDController PIDTurret; @@ -69,6 +69,7 @@ public void autonomousPeriodic(GenericRobot robot) { if (autonomousStep >= 1){ robot.getCargo(); robot.shoot(); + robot.setShooterTargetRPM(3700); } if (autonomousStep >= 1 && autonomousStep <=10){ robot.setTurretPitchPosition(.38); diff --git a/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java index 3de91cf..5b8ad8f 100644 --- a/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java +++ b/src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java @@ -20,8 +20,8 @@ public class BallCtoTerminalReturn extends GenericAutonomous { boolean time = false; double distanceC = 47.9; - double distanceTerminal = 225; - double angleC = 82.74; + double distanceTerminal = 219; + double angleC = 84.74; double rampDownDist = 10; PIDController PIDDriveStraight; @@ -92,6 +92,12 @@ public void autonomousPeriodic(GenericRobot robot) { 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); 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/command/Hang.java b/src/main/java/frc/robot/command/Hang.java index cf29de9..4b79155 100644 --- a/src/main/java/frc/robot/command/Hang.java +++ b/src/main/java/frc/robot/command/Hang.java @@ -100,7 +100,6 @@ public void step(GenericRobot robot){ } } robot.setTurretPowerPct(turretPower); - robot.raiseCollector(); switch (commandStep) { /////////////tapeAlign Code case -1: robot.resetEncoders(); @@ -214,7 +213,7 @@ else if (!robot.getFloorSensorLeft()) { rightArmPower = 0; countLeft = 0; countRight = 0; - if (System.currentTimeMillis() - startingTime >= 5000){ + if (System.currentTimeMillis() - startingTime >= 2000){ SmartDashboard.putNumber("driveOutputCurrent", robot.getDriveCurrent()); commandStep = 2; ///TODO: fix numbering } @@ -482,10 +481,10 @@ else if (!robot.getFloorSensorLeft()) { if (robot.getRoll() - level < - leveltol){ leftArmPower *= .8; } - if (Math.abs(robot.getRoll()-level) >= 3*leveltol){ + /*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 index 9592f06..9286c0a 100644 --- a/src/main/java/frc/robot/command/HangWithoutAlign.java +++ b/src/main/java/frc/robot/command/HangWithoutAlign.java @@ -360,10 +360,7 @@ public void step(GenericRobot robot){ 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/generic/Lightning.java b/src/main/java/frc/robot/generic/Lightning.java index 57ba8ca..5f57c84 100644 --- a/src/main/java/frc/robot/generic/Lightning.java +++ b/src/main/java/frc/robot/generic/Lightning.java @@ -341,7 +341,7 @@ public double encoderTurretTicksPerDegree(){ public double getAlternateTurretAngle(){ double raw = encoderTurretAlt.getPosition(); double out; - double offset = 185-45+30+5+163+15; + double offset = 78 + 15; out = (raw * 136.467) - 5.73 - offset; if (out>360) {