diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToFirstSample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToFirstSample.java index 82fae01..7ecb305 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToFirstSample.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToFirstSample.java @@ -18,7 +18,7 @@ public State tick(DebugTree debug, QQOpMode opMode) { if (lastStatus != State.RUNNING){ return lastStatus; }else{ - boolean isDoneDriving = opMode.nav.driveToPositionIN(55,-102,0); + boolean isDoneDriving = opMode.nav.driveToPositionIN(37,80,0); if (isDoneDriving) { lastStatus = State.SUCCESS; return State.SUCCESS; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToIntakePosition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToIntakePosition.java new file mode 100644 index 0000000..81c67b9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToIntakePosition.java @@ -0,0 +1,33 @@ +package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions; + +import com.ftcteams.behaviortrees.DebugTree; + +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.QQTimeoutNode; +import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode; + +public class DriveToIntakePosition extends QQTimeoutNode { + public DriveToIntakePosition(double seconds) { + super(seconds); + } + State lastStatus = State.RUNNING; + + @Override + public State tick(DebugTree debug, QQOpMode opMode) { + + opMode.telemetry.addData("location",opMode.robot.otos.getOtosPosition()); + if (lastStatus != State.RUNNING){ + return lastStatus; + }else{ + boolean isDoneDriving = opMode.nav.driveToPositionIN(52,47,0,6); + if (isDoneDriving) { + lastStatus = State.SUCCESS; + return State.SUCCESS; + } + if (hasTimedOut()) { + lastStatus = State.FAILURE; + return State.FAILURE; + } + return State.RUNNING; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToScorePosition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToScorePosition.java new file mode 100644 index 0000000..4973f2f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToScorePosition.java @@ -0,0 +1,35 @@ +package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions; + +import com.ftcteams.behaviortrees.DebugTree; + +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.QQTimeoutNode; +import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode; + +public class DriveToScorePosition extends QQTimeoutNode { + double xPosition; + public DriveToScorePosition(double seconds, double xPosition) { + super(seconds); + this.xPosition = xPosition; + } + State lastStatus = State.RUNNING; + + @Override + public State tick(DebugTree debug, QQOpMode opMode) { + + opMode.telemetry.addData("location",opMode.robot.otos.getOtosPosition()); + if (lastStatus != State.RUNNING){ + return lastStatus; + }else{ + boolean isDoneDriving = opMode.nav.driveToPositionIN(xPosition,50,0,4); + if (isDoneDriving) { + lastStatus = State.SUCCESS; + return State.SUCCESS; + } + if (hasTimedOut()) { + lastStatus = State.FAILURE; + return State.FAILURE; + } + return State.RUNNING; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToChamber.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/FirstScore.java similarity index 86% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToChamber.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/FirstScore.java index d9e9776..5e9b860 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToChamber.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/FirstScore.java @@ -5,11 +5,11 @@ import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.QQTimeoutNode; import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode; -public class DriveToChamber extends QQTimeoutNode { - public static final double FORWARD_SPEED = 0.3; +public class FirstScore extends QQTimeoutNode { + public static final double FORWARD_SPEED = 0.4; State lastStatus = State.RUNNING; - public DriveToChamber(double seconds) { + public FirstScore(double seconds) { super(seconds); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/GetReadyToPushSamples.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/GetReadyToPushSamples.java index a27e5ea..db97278 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/GetReadyToPushSamples.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/GetReadyToPushSamples.java @@ -17,7 +17,7 @@ public State tick(DebugTree debug, QQOpMode opMode) { if (lastStatus != State.RUNNING){ return lastStatus; }else{ - boolean isDoneDriving = opMode.nav.driveToPositionIN(17,-91,0); + boolean isDoneDriving = opMode.nav.driveToPositionIN(37,50,0); if (isDoneDriving) { lastStatus = State.SUCCESS; return State.SUCCESS; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/IntakeAttempt.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/IntakeAttempt.java index 32668aa..2118d09 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/IntakeAttempt.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/IntakeAttempt.java @@ -26,8 +26,7 @@ public State tick(DebugTree debug, QQOpMode opMode) { opMode.robot.mecanumDrive.move(INTAKE_SPEED,0,0); if (opMode.robot.scoringClaw.isClawClosed()){ opMode.robot.mecanumDrive.stop(); - opMode.robot.intakeClaw.open(); - opMode.robot.intakeArm.goToAutoDropPos(); + opMode.robot.intakeClaw.open();; lastStatus = State.SUCCESS; return State.SUCCESS; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/MoveForwardForTime.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/MoveForwardForTime.java new file mode 100644 index 0000000..4ab5ebd --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/MoveForwardForTime.java @@ -0,0 +1,30 @@ +package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions; + +import com.ftcteams.behaviortrees.DebugTree; + +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.QQTimeoutNode; +import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode; + +public class MoveForwardForTime extends QQTimeoutNode { + double speed; + State lastStatus = State.RUNNING; + public MoveForwardForTime(double seconds, double speed) { + super(seconds); + this.speed = speed; + } + + @Override + public State tick(DebugTree debug, QQOpMode opMode) { + if (lastStatus != State.RUNNING){ + return lastStatus;} + else { + opMode.robot.mecanumDrive.move(-speed,0,0); + if (hasTimedOut()){ + opMode.robot.mecanumDrive.stop(); + lastStatus = State.SUCCESS; + return State.SUCCESS; + } + return State.RUNNING; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/MoveRightForTime.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/MoveRightForTime.java new file mode 100644 index 0000000..5581a66 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/MoveRightForTime.java @@ -0,0 +1,31 @@ +package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions; + +import com.ftcteams.behaviortrees.DebugTree; + +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.QQTimeoutNode; +import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode; + +public class MoveRightForTime extends QQTimeoutNode { + double speed; + State lastStatus = State.RUNNING; + public MoveRightForTime(double seconds, double speed) { + super(seconds); + this.speed = speed; + } + + @Override + public State tick(DebugTree debug, QQOpMode opMode) { + if (lastStatus != State.RUNNING){ + return lastStatus;} + else { + opMode.robot.mecanumDrive.move(0,speed,0); + if (hasTimedOut()){ + opMode.robot.mecanumDrive.stop(); + lastStatus = State.SUCCESS; + return State.SUCCESS; + + } + return State.RUNNING; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Park.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Park.java index d582218..deb9cf3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Park.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Park.java @@ -18,7 +18,7 @@ public State tick(DebugTree debug, QQOpMode opMode) { if (lastStatus != State.RUNNING){ return lastStatus; }else{ - boolean isDoneDriving = opMode.nav.driveToPositionIN(12,-90.5,0); + boolean isDoneDriving = opMode.nav.driveToPositionIN(45,37,0); if (isDoneDriving) { lastStatus = State.SUCCESS; return State.SUCCESS; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/PushFirstSample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/PushFirstSample.java index 3b411f5..e5f0734 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/PushFirstSample.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/PushFirstSample.java @@ -18,7 +18,7 @@ public State tick(DebugTree debug, QQOpMode opMode) { if (lastStatus != State.RUNNING){ return lastStatus; }else{ - boolean isDoneDriving = opMode.nav.driveToPositionIN(14,-88,90); + boolean isDoneDriving = opMode.nav.driveToPositionIN(50,80,0); if (isDoneDriving) { lastStatus = State.SUCCESS; return State.SUCCESS; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/PushSamplesIn.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/PushSamplesIn.java new file mode 100644 index 0000000..2567d8e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/PushSamplesIn.java @@ -0,0 +1,32 @@ +package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions; + +import com.ftcteams.behaviortrees.DebugTree; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.QQTimeoutNode; +import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode; + +public class PushSamplesIn extends QQTimeoutNode { + public PushSamplesIn(double seconds) { + super(seconds); + } + State lastStatus = State.RUNNING; + + @Override + public State tick(DebugTree debug, QQOpMode opMode) { + opMode.telemetry.addData("location",opMode.robot.otos.getOtosPosition()); + if (lastStatus != State.RUNNING){ + return lastStatus; + }else{ + boolean isDoneDriving = opMode.nav.driveToPositionIN(opMode.robot.otos.getOtosPosition().x, 47,0); + if (isDoneDriving) { + lastStatus = State.SUCCESS; + return State.SUCCESS; + }if (hasTimedOut()) { + lastStatus = State.FAILURE; + return State.FAILURE; + } + return State.RUNNING; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/PushSecondSample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/PushSecondSample.java index 6a546ba..d8652db 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/PushSecondSample.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/PushSecondSample.java @@ -18,7 +18,7 @@ public State tick(DebugTree debug, QQOpMode opMode) { if (lastStatus != State.RUNNING){ return lastStatus; }else{ - boolean isDoneDriving = opMode.nav.driveToPositionIN(14,-88,90); + boolean isDoneDriving = opMode.nav.driveToPositionIN(61.5,85,0); if (isDoneDriving) { lastStatus = State.SUCCESS; return State.SUCCESS; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/PushThirdSample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/PushThirdSample.java new file mode 100644 index 0000000..06c8fa0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/PushThirdSample.java @@ -0,0 +1,33 @@ +package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions; + +import com.ftcteams.behaviortrees.DebugTree; + +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.QQTimeoutNode; +import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode; + +public class PushThirdSample extends QQTimeoutNode { + public PushThirdSample(double seconds) { + super(seconds); + } + State lastStatus = State.RUNNING; + + @Override + public State tick(DebugTree debug, QQOpMode opMode) { + + opMode.telemetry.addData("location",opMode.robot.otos.getOtosPosition()); + if (lastStatus != State.RUNNING){ + return lastStatus; + }else{ + boolean isDoneDriving = opMode.nav.driveToPositionIN(59,82,0); + if (isDoneDriving) { + lastStatus = State.SUCCESS; + return State.SUCCESS; + } + if (hasTimedOut()) { + lastStatus = State.FAILURE; + return State.FAILURE; + } + return State.RUNNING; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/StandardScore.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/StandardScore.java new file mode 100644 index 0000000..9c5b3e6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/StandardScore.java @@ -0,0 +1,38 @@ +package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions; + +import com.ftcteams.behaviortrees.DebugTree; + +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.QQTimeoutNode; +import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode; + +public class StandardScore extends QQTimeoutNode { + public static final double FORWARD_SPEED = 0.3; + public static final double RIGHT_SPEED = 0.1; + State lastStatus = State.RUNNING; + + public StandardScore(double seconds) { + super(seconds); + } + + @Override + public State tick(DebugTree debug, QQOpMode opMode) { + if (lastStatus != State.RUNNING){ + return lastStatus; + }else{ + if(hasTimedOut()){ + opMode.robot.mecanumDrive.stop(); + lastStatus = State.FAILURE; + return State.FAILURE; + } + opMode.robot.mecanumDrive.move(FORWARD_SPEED,RIGHT_SPEED,0); + if (opMode.robot.scoringClaw.isClawOpen()){ + opMode.robot.mecanumDrive.stop(); + lastStatus = State.SUCCESS; + return State.SUCCESS; + + } + return State.RUNNING; + } + } +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/Cycle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/Cycle.java new file mode 100644 index 0000000..6e6ea57 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/Cycle.java @@ -0,0 +1,47 @@ +/* Tree TwoSpecimenTree for 16072 generated by http://behaviortrees.ftcteams.com */ + package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees; + +import com.ftcteams.behaviortrees.Node; +import com.ftcteams.behaviortrees.Parallel; +import com.ftcteams.behaviortrees.Sequence; + +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ArmToIntake; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ArmToScore; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveToIntakePosition; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveToScorePosition; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MoveForwardForTime; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.StandardScore; + + +public class Cycle { + public static final int TIMEOUT_SECONDS = 10; + + + + public static Node root(double xScorePosition){ + return new Sequence( + Intake.root(), + new MoveForwardForTime(.2,-.25), + new Parallel(2, + new DriveToScorePosition(TIMEOUT_SECONDS, xScorePosition), + new ArmToScore(TIMEOUT_SECONDS)), + new StandardScore(TIMEOUT_SECONDS), + new MoveForwardForTime(0.25,1), + new Parallel(2, + new DriveToIntakePosition(TIMEOUT_SECONDS), + new ArmToIntake(TIMEOUT_SECONDS))); + + } +} + +/* TREE +? +| -> +| | [FisrtScore] +| | [DriveToIntakePosition] +| | [Intake] +| | [DrivetoScorePosition] +| | [StandardScore] +| | [Park] +| [Park] + */ \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/FourSpecimenTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/FourSpecimenTree.java new file mode 100644 index 0000000..a703865 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/FourSpecimenTree.java @@ -0,0 +1,50 @@ +/* Tree TwoSpecimenTree for 16072 generated by http://behaviortrees.ftcteams.com */ + package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees; + +import com.ftcteams.behaviortrees.Node; +import com.ftcteams.behaviortrees.Failover; +import com.ftcteams.behaviortrees.Parallel; +import com.ftcteams.behaviortrees.Sequence; + +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ArmToIntake; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.FirstScore; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveToIntakePosition; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Park; + + + +public class FourSpecimenTree { + public static final int TIMEOUT_SECONDS = 10; + + public static Node root(){ + return new Failover( + new Sequence( + new FirstScore(TIMEOUT_SECONDS), + new Parallel(2, + new DriveToIntakePosition(TIMEOUT_SECONDS), + new ArmToIntake(TIMEOUT_SECONDS)), + Cycle.root(10), + Cycle.root(10), + Cycle.root(10), + Cycle.root(10), + new Parallel(2, + new Park(TIMEOUT_SECONDS), + new ArmToIntake(TIMEOUT_SECONDS)) + ), + new Parallel(2, + new Park(TIMEOUT_SECONDS), + new ArmToIntake(TIMEOUT_SECONDS))); + } +} + +/* TREE +? +| -> +| | [FisrtScore] +| | [DriveToIntakePosition] +| | [Intake] +| | [DrivetoScorePosition] +| | [StandardScore] +| | [Park] +| [Park] + */ \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/Intake.java index 82431db..5293d01 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/Intake.java @@ -4,9 +4,9 @@ import com.ftcteams.behaviortrees.Failover; import com.ftcteams.behaviortrees.Node; import com.ftcteams.behaviortrees.Sequence; + import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.IntakeAttempt; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReadyToIntakeOne; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReadyToIntakeTwo; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MoveForwardForTime; public class Intake { @@ -16,7 +16,16 @@ public static Node root(){ return new Failover( new IntakeAttempt(INTAKE_TIMEOUT_SECONDS), new Sequence( - new ReadyToIntakeTwo(MOVEMENT_TIMEOUT_SECONDS), + new MoveForwardForTime(0.25,-.5), + new IntakeAttempt(INTAKE_TIMEOUT_SECONDS)), + new Sequence( + new MoveForwardForTime(0.25,-.5), + new IntakeAttempt(INTAKE_TIMEOUT_SECONDS)), + new Sequence( + new MoveForwardForTime(0.25,-.5), + new IntakeAttempt(INTAKE_TIMEOUT_SECONDS)), + new Sequence( + new MoveForwardForTime(0.25,-.5), new IntakeAttempt(INTAKE_TIMEOUT_SECONDS))); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/PreloadAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/PreloadAuto.java deleted file mode 100644 index 60fc655..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/PreloadAuto.java +++ /dev/null @@ -1,45 +0,0 @@ -package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees; - -/* Tree PreloadAuto for 16072 generated by http://behaviortrees.ftcteams.com */ - -import com.ftcteams.behaviortrees.Node; -import com.ftcteams.behaviortrees.Failover; -import com.ftcteams.behaviortrees.Parallel; -import com.ftcteams.behaviortrees.Sequence; - -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ArmToIntake; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.BehindChamber; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveToChamber; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.GetReadyToPushSamples; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReadyToIntakeOne; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.WaitForClawOpen; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Park; - - -public class PreloadAuto { - - public static final int TIMEOUT_SECONDS = 10; - - public static Node root(){ - return new Failover( - new Sequence( - new DriveToChamber(TIMEOUT_SECONDS), - new WaitForClawOpen(TIMEOUT_SECONDS), - new BehindChamber(TIMEOUT_SECONDS), - new GetReadyToPushSamples(TIMEOUT_SECONDS), - PushSamples.root(), - new Parallel(2, - new ArmToIntake(TIMEOUT_SECONDS), - new ReadyToIntakeOne(TIMEOUT_SECONDS))), - new Park(TIMEOUT_SECONDS)); - } -} - -/* TREE -? -| -> -| | [DriveToChamber] -| | [Score] -| | [Park] -| [Park] - */ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpecimenCycleAutoTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpecimenCycleAutoTree.java index c75db67..ac2b8ce 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpecimenCycleAutoTree.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpecimenCycleAutoTree.java @@ -9,11 +9,10 @@ import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ArmToIntake; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ArmToScore; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ArmToScored; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.BehindChamber; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.BehindChamberForSecondScore; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.BehindChamberForThirdScore; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveToChamber; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.FirstScore; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.IntakeArmOut; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Park; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReadyToIntakeOne; @@ -30,7 +29,7 @@ public class SpecimenCycleAutoTree { public static Node root(){ return new Failover( new Sequence( - new DriveToChamber(TIMEOUT_SECONDS), + new FirstScore(TIMEOUT_SECONDS), new WaitForClawOpen(TIMEOUT_SECONDS), new Parallel(1, new IntakeArmOut(1), @@ -47,7 +46,7 @@ public static Node root(){ new ReadyToIntakeTwo(TIMEOUT_SECONDS), new ArmToScore(TIMEOUT_SECONDS)), new BehindChamberForSecondScore(TIMEOUT_SECONDS), - new DriveToChamber(TIMEOUT_SECONDS), + new FirstScore(TIMEOUT_SECONDS), new WaitForClawOpen(TIMEOUT_SECONDS), new BehindChamber(TIMEOUT_SECONDS), new Parallel(2, @@ -58,7 +57,7 @@ public static Node root(){ new ReadyToIntakeOne(TIMEOUT_SECONDS), new ArmToScore(TIMEOUT_SECONDS)), new BehindChamberForThirdScore(TIMEOUT_SECONDS), - new DriveToChamber(TIMEOUT_SECONDS), + new FirstScore(TIMEOUT_SECONDS), new WaitForClawOpen(TIMEOUT_SECONDS), new Park(TIMEOUT_SECONDS)), diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpecimenTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpecimenTree.java new file mode 100644 index 0000000..99db301 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpecimenTree.java @@ -0,0 +1,65 @@ +/* Tree TwoSpecimenTree for 16072 generated by http://behaviortrees.ftcteams.com */ + package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees; + +import com.ftcteams.behaviortrees.Failover; +import com.ftcteams.behaviortrees.Node; +import com.ftcteams.behaviortrees.Parallel; +import com.ftcteams.behaviortrees.Sequence; + +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ArmToIntake; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveToFirstSample; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.FirstScore; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.GetReadyToPushSamples; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MoveForwardForTime; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Park; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.PushFirstSample; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.PushSamplesIn; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.PushSecondSample; + + +public class SpecimenTree { + public static final int TIMEOUT_SECONDS = 8; + + public static Node root(){ + return new Failover( + new Sequence( + new FirstScore(TIMEOUT_SECONDS), + new MoveForwardForTime(0.25,1), + new Parallel(2, + new ArmToIntake(TIMEOUT_SECONDS), + new GetReadyToPushSamples(TIMEOUT_SECONDS) + ), + new DriveToFirstSample(TIMEOUT_SECONDS), + new PushFirstSample(TIMEOUT_SECONDS), + new PushSamplesIn(TIMEOUT_SECONDS), + new PushFirstSample(TIMEOUT_SECONDS), + new PushSecondSample(TIMEOUT_SECONDS), + new MoveForwardForTime(0.5,1), + /* new PushSecondSample(TIMEOUT_SECONDS), + new MoveRightForTime(.2,1), //square on wall + new PushSamplesIn(TIMEOUT_SECONDS),*/ + Cycle.root(2), + Cycle.root(5), + Cycle.root(7), + //Cycle.root(), + new Parallel(2, + new Park(TIMEOUT_SECONDS), + new ArmToIntake(TIMEOUT_SECONDS)) + ), + new Parallel(2, + new Park(TIMEOUT_SECONDS), + new ArmToIntake(TIMEOUT_SECONDS))); + } +} + +/* TREE +? +| -> +| | [FisrtScore] +| | [DriveToIntakePosition] +| | [Intake] +| | [DrivetoScorePosition] +| | [StandardScore] +| | [Park] +| [Park] + */ \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/TestAutoTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/TestAutoTree.java new file mode 100644 index 0000000..393d731 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/TestAutoTree.java @@ -0,0 +1,20 @@ +package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees; + +/* Tree PreloadAuto for 16072 generated by http://behaviortrees.ftcteams.com */ + +import com.ftcteams.behaviortrees.Node; +import com.ftcteams.behaviortrees.Sequence; + +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Park; + + +public class TestAutoTree { + + public static final int TIMEOUT_SECONDS = 10; + + public static Node root(){ + return new Sequence( + new Park(TIMEOUT_SECONDS)); +}} + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/ThreeSpecimenAutoTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/ThreeSpecimenAutoTree.java deleted file mode 100644 index e080b5b..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/ThreeSpecimenAutoTree.java +++ /dev/null @@ -1,72 +0,0 @@ -package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees; - -/* Tree PreloadAuto for 16072 generated by http://behaviortrees.ftcteams.com */ - -import com.ftcteams.behaviortrees.Failover; -import com.ftcteams.behaviortrees.Node; -import com.ftcteams.behaviortrees.Parallel; -import com.ftcteams.behaviortrees.Sequence; - -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ArmToIntake; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ArmToScore; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.BehindChamber; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.BehindChamberForSecondScore; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.BehindChamberForThirdScore; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveToChamber; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveToFirstSample; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveToSamples; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.GetReadyToPushSamples; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Park; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReadyToIntakeOne; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ScoreClawOpen; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.WaitForClawOpen; - - -public class ThreeSpecimenAutoTree { - - public static final int TIMEOUT_SECONDS = 10; - - public static Node root(){ - return new Failover( - new Sequence( - new DriveToChamber(TIMEOUT_SECONDS), - new WaitForClawOpen(TIMEOUT_SECONDS), - new BehindChamber(TIMEOUT_SECONDS), - new GetReadyToPushSamples(TIMEOUT_SECONDS), - new DriveToSamples(TIMEOUT_SECONDS), - new DriveToFirstSample(TIMEOUT_SECONDS), - new Parallel(2, - new ArmToIntake(TIMEOUT_SECONDS), - new ReadyToIntakeOne(TIMEOUT_SECONDS)), - Intake.root(), - new Parallel(2, - new ReadyToIntakeOne(TIMEOUT_SECONDS), - new ArmToScore(TIMEOUT_SECONDS)), - new BehindChamberForSecondScore(TIMEOUT_SECONDS), - new DriveToChamber(TIMEOUT_SECONDS), - new WaitForClawOpen(TIMEOUT_SECONDS), - new Parallel(2, - new ReadyToIntakeOne(TIMEOUT_SECONDS), - new ArmToIntake(TIMEOUT_SECONDS)), - Intake.root(), - new Parallel(2, - new ReadyToIntakeOne(TIMEOUT_SECONDS), - new ArmToScore(TIMEOUT_SECONDS)), - new BehindChamberForThirdScore(TIMEOUT_SECONDS), - new DriveToChamber(TIMEOUT_SECONDS), - new WaitForClawOpen(TIMEOUT_SECONDS)), - new Parallel(3, - new Park(TIMEOUT_SECONDS), - new ScoreClawOpen(2), - new ArmToIntake(TIMEOUT_SECONDS))); - } -} - -/* TREE -? -| -> -| | [DriveToChamber] -| | [Score] -| | [Park] -| [Park] - */ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/TwoSpecimenAutoTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/TwoSpecimenAutoTree.java deleted file mode 100644 index d3f0c15..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/TwoSpecimenAutoTree.java +++ /dev/null @@ -1,51 +0,0 @@ -package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees; - -/* Tree PreloadAuto for 16072 generated by http://behaviortrees.ftcteams.com */ - -import com.ftcteams.behaviortrees.Failover; -import com.ftcteams.behaviortrees.Node; -import com.ftcteams.behaviortrees.Parallel; -import com.ftcteams.behaviortrees.Sequence; - -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ArmToIntake; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.BehindChamber; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveToChamber; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Park; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReadyToIntakeOne; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ScoreClawOpen; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.WaitForClawOpen; - - -public class TwoSpecimenAutoTree { - - public static final int TIMEOUT_SECONDS = 10; - - public static Node root(){ - return new Failover( - new Sequence( - new DriveToChamber(TIMEOUT_SECONDS), - new WaitForClawOpen(TIMEOUT_SECONDS), - new BehindChamber(TIMEOUT_SECONDS), - new Parallel(2, - new ArmToIntake(TIMEOUT_SECONDS), - new ReadyToIntakeOne(TIMEOUT_SECONDS)), - Intake.root(), - new BehindChamber(TIMEOUT_SECONDS), - new DriveToChamber(TIMEOUT_SECONDS), - new WaitForClawOpen(TIMEOUT_SECONDS), - new Parallel(TIMEOUT_SECONDS)), - new Parallel(3, - new Park(TIMEOUT_SECONDS), - new ScoreClawOpen(2), - new ArmToIntake(TIMEOUT_SECONDS))); - } -} - -/* TREE -? -| -> -| | [DriveToChamber] -| | [Score] -| | [Park] -| [Park] - */ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Limelight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Limelight.java new file mode 100644 index 0000000..13fd6a5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Limelight.java @@ -0,0 +1,72 @@ +package org.firstinspires.ftc.teamcode.ftc16072.Mechanisms; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.firstinspires.ftc.teamcode.ftc16072.Tests.QQTest; +import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestLimelight; + +import java.util.Collections; +import java.util.List; + +public class Limelight extends QQMechanism{ + private Limelight3A limelight; + LLResult llResult; + private Pose3D lastValidPos = new Pose3D( + //arbitrary starting pose + new Position(DistanceUnit.INCH, 0,0,0,0), + new YawPitchRollAngles(AngleUnit.DEGREES, 0, 0, 0, 0)); + private Pose3D getBotPose() { + llResult = limelight.getLatestResult(); + if((llResult != null) && llResult.isValid()){ + lastValidPos = llResult.getBotpose_MT2(); + //lastValidPos = llResult.getBotpose(); + } + return lastValidPos; + } + @Override + public void init(HardwareMap hwMap) { + limelight = hwMap.get(Limelight3A.class, "limelight"); + } + public boolean isAprilTagSeen(){ + return ((llResult != null) && llResult.isValid()); + } + + @Override + public void update(Telemetry telemetry){ + super.update(telemetry); + getBotPose(); + telemetry.addData("april tag seen", isAprilTagSeen()); + } + public void start(){ + limelight.start(); + } + public void stop(){ + limelight.stop(); + } + public Pose3D getRobotPosition(){ + return getBotPose(); + } + public double getRobotPositionX(DistanceUnit distanceUnit) { + return distanceUnit.fromMeters(getBotPose().getPosition().x); + } + public double getRobotPositionY(DistanceUnit distanceUnit) { + return distanceUnit.fromMeters(getBotPose().getPosition().y); + } + public void setYaw(AngleUnit angleUnit, double yaw){ + limelight.updateRobotOrientation(angleUnit.toDegrees(yaw)); + } + + + + @Override + public List getTests() {return Collections.singletonList(new TestLimelight("Limelight attached ", limelight));} +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/OpticalTrackingOdometrySensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/OpticalTrackingOdometrySensor.java index f19d222..7c328b1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/OpticalTrackingOdometrySensor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/OpticalTrackingOdometrySensor.java @@ -21,7 +21,7 @@ public void init(HardwareMap hwMap) { otos.setAngularUnit(AngleUnit.DEGREES); otos.setLinearUnit(DistanceUnit.INCH); otos.calibrateImu(); - SparkFunOTOS.Pose2D offset = new SparkFunOTOS.Pose2D(X_POSITION,0,0); + SparkFunOTOS.Pose2D offset = new SparkFunOTOS.Pose2D(0,0,90); otos.setOffset(offset); otos.resetTracking(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java index a1e1b6d..4e3e1c8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java @@ -22,7 +22,8 @@ public class ScoreArm extends QQMechanism{ public static final double TEST_SPEED = 0.55; public static final int CLAW_RELEASE_OFFSET = 250; public static final double SCORE_POWER = -0.8; - public static final double STALL_CURRENT = 4.1; + public static final double STALL_CURRENT = 8; + public static final int TOLERANCE_THRESHOLD = 100; DcMotorEx leftMotor; DcMotorEx rightMotor; TouchSensor limitSwitch; @@ -34,15 +35,16 @@ public class ScoreArm extends QQMechanism{ public static double kF = 0; public static double max = 1.0; public static double min = -1.0; - boolean chamberContacted; + boolean wasChamberContacted; protected int currentPos; public int desiredPos; - public double motorPower; boolean isScoring; - boolean wasChamberContacted; + double lastEncoderPos; + + boolean encoderStalled; public static int INTAKE_POSITION = 0; - public static int SCORING_POSITION = 350; + public static int SCORING_POSITION = 450; public static int PLACING_POSITION = 830; public static int MOVING_POSITION = 450; public static int INIT_POSITION = 240; @@ -86,6 +88,7 @@ public void manualPositionChange(int changeAmount){ @Override public void update(Telemetry telemetry){ + boolean newChamberContacted = rightChamberContact.isPressed() || leftChamberContact.isPressed(); if(limitSwitch.isPressed()) { leftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); leftMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); @@ -95,25 +98,31 @@ public void update(Telemetry telemetry){ if (desiredPos < 0){ desiredPos = 0; } - }else if (chamberContacted && !wasChamberContacted){ + }else if (newChamberContacted && !wasChamberContacted){ isScoring = true; goToScoring(); } + wasChamberContacted = newChamberContacted; currentPos = (leftMotor.getCurrentPosition() + rightMotor.getCurrentPosition())/2;//average left and right speeds double motorPower = pidfController.calculate(desiredPos,currentPos); - this.motorPower = motorPower; if (isScoring){ + if (lastEncoderPos == currentPos){ + encoderStalled = true; + } + lastEncoderPos = currentPos; motorPower = SCORE_POWER; + }else { + lastEncoderPos = 0; + encoderStalled = false; } leftMotor.setPower(motorPower); rightMotor.setPower(motorPower); - if (Math.abs(desiredPos - currentPos) <= 30){ + if (Math.abs(desiredPos - currentPos) <= TOLERANCE_THRESHOLD){ isWithinTolerance = true; }else {isWithinTolerance = false;} - chamberContacted = rightChamberContact.isPressed() || leftChamberContact.isPressed(); //pidfController.updateConstants(kP,kI,kD,kF,max,min); telemetry.addData("curerent pos",currentPos); telemetry.addData("desired pos",desiredPos); @@ -121,14 +130,12 @@ public void update(Telemetry telemetry){ telemetry.addData("left motor current", leftMotor.getCurrent(CurrentUnit.AMPS)); telemetry.addData("right motor current", rightMotor.getCurrent(CurrentUnit.AMPS)); telemetry.addData("Is stalled", isStalling()); - - wasChamberContacted = chamberContacted; } public void setNotScoring(){ isScoring = false; } public boolean isChamberContacted(){ - return chamberContacted; + return wasChamberContacted; } public int getCurrentPos() { @@ -143,11 +150,9 @@ public boolean isLimitSwitchPressed(){ public boolean getIsWithinTolerence(){return isWithinTolerance;} + public boolean isStalling(){ - if((leftMotor.getCurrent(CurrentUnit.AMPS) > STALL_CURRENT) || (rightMotor.getCurrent(CurrentUnit.AMPS) > STALL_CURRENT)){ - return true; - } - return false; + return (((leftMotor.getCurrent(CurrentUnit.AMPS) > STALL_CURRENT) || (rightMotor.getCurrent(CurrentUnit.AMPS) > STALL_CURRENT))||encoderStalled); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoringClaw.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoringClaw.java index ccc4553..f3947a4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoringClaw.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoringClaw.java @@ -22,8 +22,8 @@ @Config public class ScoringClaw extends QQMechanism { public static final int GRABBABLE_DISTANCE_CM = 3; - public static final double OPEN_TIME = 0.5; - public static final double CLOSED_TIME = 0.25; + public static final double OPEN_TIME = 0.2; + public static final double CLOSED_TIME = 0.3; public static double CLAW_CLOSE_POSITION = 0.5; public static double CLAW_OPEN_POSITION = 0; static boolean hasColor = false; @@ -87,6 +87,9 @@ public void close() { clawServo.setPosition(CLAW_CLOSE_POSITION); closedTimer.reset(); } + public void resetHasColor(){ + hasColor = false; + } public boolean isColorRed(){ if (!hasColor){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ThreeSpecimenAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/Auto.java similarity index 59% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ThreeSpecimenAuto.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/Auto.java index 245a987..450f7ac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ThreeSpecimenAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/Auto.java @@ -1,29 +1,34 @@ package org.firstinspires.ftc.teamcode.ftc16072.OpModes; +import android.util.Log; + import com.ftcteams.behaviortrees.DebugTree; import com.ftcteams.behaviortrees.Node; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.util.ElapsedTime; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees.ThreeSpecimenAutoTree; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees.TwoSpecimenAutoTree; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees.SpecimenTree; @Autonomous -public class ThreeSpecimenAuto extends QQOpMode{ +public class Auto extends QQOpMode{ boolean clawWasClosed; boolean test; boolean done; - Node root = ThreeSpecimenAutoTree.root(); + Node root = SpecimenTree.root(); DebugTree debugTree = new DebugTree(); ElapsedTime moveTimer = new ElapsedTime(); double INIT_MOVE_TIME_SECONDS = 1; double INIT_MOVE_SPEED = -0.2; + boolean switchWasPressed; @Override public void init(){ + robot.scoringClaw.resetHasColor(); super.init(); moveTimer.reset(); robot.scoreArm.goToInit(); robot.scoringClaw.close(); + robot.intakeArm.goToDropPos(); } @Override @@ -38,25 +43,44 @@ public void init_loop() { } public void start(){ - robot.otos.setOtosPosition(7,-61.5,0); + super.start(); + robot.otos.setOtosPosition(10,32,0); } public void loop() { super.loop(); + double degreesYaw = robot.controlHub.getYaw(AngleUnit.DEGREES); + if (isAllianceRed){ + degreesYaw = AngleUnit.normalizeDegrees(degreesYaw + 180); + } + robot.limelight.setYaw(AngleUnit.DEGREES, degreesYaw); if (robot.scoringClaw.isClawClosed() && !clawWasClosed) { robot.intakeClaw.open(); robot.scoreArm.goToPlace(); + test = true; + } + if(robot.scoreArm.isStalling()){ + robot.scoringClaw.open(); + } + if (robot.scoringClaw.isClawOpen()){ + test = false; } if (robot.scoringClaw.isScoreSwitchPressed()){ robot.scoreArm.setNotScoring(); } + if (robot.scoreArm.isLimitSwitchPressed() && !switchWasPressed){ + robot.scoringClaw.open(); + } if(!done){ + // debugTree.reset(); Node.State state = root.tick(debugTree, this); if(state == Node.State.SUCCESS){ done = true; } +// Log.d("QQ", "DT:" + debugTree.toString()); } clawWasClosed = robot.scoringClaw.isClawClosed(); + switchWasPressed = robot.scoreArm.isLimitSwitchPressed(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/BlueCloseParkAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/BlueCloseParkAuto.java deleted file mode 100644 index 7b2625e..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/BlueCloseParkAuto.java +++ /dev/null @@ -1,28 +0,0 @@ -package org.firstinspires.ftc.teamcode.ftc16072.OpModes; - -public class BlueCloseParkAuto extends QQOpMode{ - private double step = 0; - public void init(){ - super.init(); - robot.otos.setOtosPosition(-13.5,61.5,0); - } - public void loop(){ - super.loop(); - if(step == 0){ - boolean doneDriving = nav.driveToPositionIN(-13.5,60,0); - if(doneDriving){ - step = 1; - } - }else if(step == 1){ - boolean doneDriving = nav.driveToPositionIN(-60,60,0); - if(doneDriving){ - step = 2; - }if(step == 2){ - telemetry.addData("parked", 0); - robot.scoreArm.goToIntake(); - - } - } - } -} - diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/BlueFarParkAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/BlueFarParkAuto.java deleted file mode 100644 index 1911778..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/BlueFarParkAuto.java +++ /dev/null @@ -1,27 +0,0 @@ -package org.firstinspires.ftc.teamcode.ftc16072.OpModes; - -public class BlueFarParkAuto extends QQOpMode{ - private double step = 0; - public void init(){ - super.init(); - robot.otos.setOtosPosition(62,61.5,0); - } - public void loop(){ - super.loop(); - if(step == 0){ - boolean doneDriving = nav.driveToPositionIN(62,60,0); - if(doneDriving){ - step = 1; - } - }else if(step == 1){ - boolean doneDriving = nav.driveToPositionIN(-31.5,60,0); - if(doneDriving){ - step = 2; - }if(step == 2){ - telemetry.addData("parked", 0); - robot.scoreArm.goToIntake(); - } - } - } -} - diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/CloseParkAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/CloseParkAuto.java deleted file mode 100644 index 809bad7..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/CloseParkAuto.java +++ /dev/null @@ -1,37 +0,0 @@ -package org.firstinspires.ftc.teamcode.ftc16072.OpModes; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.util.ElapsedTime; - -@Autonomous -public class CloseParkAuto extends QQOpMode{ - private final int autodelay = 0; - - ElapsedTime elapsedTime = new ElapsedTime(); - private double step = 0; - public void init(){ - super.init(); - robot.otos.setOtosPosition(13.5,-61.5,0); - robot.scoringClaw.close(); - } - public void start(){ - elapsedTime.reset(); - } - public void loop(){ - super.loop(); - if(step == 0){ - if(elapsedTime.seconds() > autodelay){ - step = 1; - } - } - else if(step == 1){ - boolean doneDriving = nav.driveToPositionIN(-31.5,-60,0); - if(doneDriving){ - step = 2; - } - }else if(step == 2){ - robot.scoreArm.goToIntake(); - } - } - } - diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/FarParkAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/FarParkAuto.java deleted file mode 100644 index b3146db..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/FarParkAuto.java +++ /dev/null @@ -1,38 +0,0 @@ -package org.firstinspires.ftc.teamcode.ftc16072.OpModes; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.util.ElapsedTime; - -@Autonomous -public class FarParkAuto extends QQOpMode{ - private double step = -1; - ElapsedTime elapsedTime = new ElapsedTime(); - public final int autodelay = 23; - public void init(){ - super.init(); - robot.otos.setOtosPosition(-31.5,-61.5,0); - } - public void start(){ - elapsedTime.reset(); - } - public void loop(){ - if(step == 0){ - if(elapsedTime.seconds() > autodelay){ - step = 1; - } - } - else if(step == 1){ - boolean doneDriving = nav.driveToPositionIN(-31.5,-60,0); - if(doneDriving){ - step = 2; - } - }else if(step == 2){ - boolean doneDriving = nav.driveToPositionIN(62,-60,0); - if(doneDriving){ - step = 3; - }if(step == 3){ - robot.scoreArm.goToIntake(); - } - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/FieldRelativeDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/FieldRelativeDrive.java index 91585d2..c50f27e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/FieldRelativeDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/FieldRelativeDrive.java @@ -2,6 +2,9 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + @TeleOp() public class FieldRelativeDrive extends QQOpMode{ @@ -9,6 +12,7 @@ public class FieldRelativeDrive extends QQOpMode{ @Override public void init(){ super.init(); + robot.limelight.start(); } @Override @@ -16,5 +20,10 @@ public void loop() { telemetry.addData("xPosition", robot.otos.getOtosPosition().x); telemetry.addData("yPosition", robot.otos.getOtosPosition().y); nav.driveFieldRelative(-gamepad1.left_stick_y,gamepad1.left_stick_x,gamepad1.right_stick_x); + robot.limelight.setYaw(AngleUnit.DEGREES, robot.controlHub.getYaw(AngleUnit.DEGREES)); + telemetry.addData("bot_pose", robot.limelight.getRobotPosition().toString()); + telemetry.addData("limelight_x", robot.limelight.getRobotPositionX(DistanceUnit.INCH)); + telemetry.addData("limelight_y", robot.limelight.getRobotPositionY(DistanceUnit.INCH)); + telemetry.addData("april tag seen",robot.limelight.isAprilTagSeen()); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/Inspection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/Inspection.java index 1eb9bec..ee5a24d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/Inspection.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/Inspection.java @@ -1,13 +1,9 @@ package org.firstinspires.ftc.teamcode.ftc16072.OpModes; import com.ftcteams.behaviortrees.DebugTree; -import com.ftcteams.behaviortrees.Node; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.util.ElapsedTime; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees.TwoSpecimenAutoTree; - @TeleOp public class Inspection extends QQOpMode{ boolean clawWasClosed; @@ -31,6 +27,7 @@ public void init_loop() { } public void start(){ + super.start(); robot.otos.setOtosPosition(7,-61.5,0); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NavigationPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NavigationPIDTuner.java index bce5b67..45e36db 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NavigationPIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NavigationPIDTuner.java @@ -4,6 +4,8 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + @TeleOp public class NavigationPIDTuner extends QQOpMode{ private final int autodelay = 0; @@ -16,22 +18,23 @@ public class NavigationPIDTuner extends QQOpMode{ boolean clawWasClosed; public void init(){ super.init(); - robot.otos.setOtosPosition(7,-61.5,0); + robot.otos.setOtosPosition(robot.limelight.getRobotPositionX(DistanceUnit.INCH),robot.limelight.getRobotPositionY(DistanceUnit.INCH),0); robot.scoringClaw.close(); } public void start(){ + super.start(); elapsedTime.reset(); } public void loop(){ super.loop(); if(step == 0){ - boolean isDoneDriving = nav.driveToPositionIN(7.5, -37.5,0); + boolean isDoneDriving = nav.driveToPositionIN(52,47,0); if (isDoneDriving){ step = 1; } }else if(step == 1){ - boolean isDoneDriving = nav.driveToPositionIN(7.5,-85.5,0); + boolean isDoneDriving = nav.driveToPositionIN(15,50,0); if (isDoneDriving){ step = 0; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java index d01c4e0..a8080d2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java @@ -27,6 +27,7 @@ public void init() { } public void start(){ + super.start(); robot.intakeArm.goToDropPos(); } @@ -106,7 +107,7 @@ public void loop() { robot.scoreArm.goToScoring(); if (!robot.intakeArm.isArmIn()){ robot.intakeArm.goToDropPos(); - robot.intakeClaw.wristDrop(); + robot.intakeClaw.wristTransfer(); }else { robot.intakeArm.goToIntake(); robot.intakeClaw.wristTransfer(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/TwoSpecimenAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/OldAuto.java similarity index 84% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/TwoSpecimenAuto.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/OldAuto.java index 7381589..5c2e4b0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/TwoSpecimenAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/OldAuto.java @@ -2,18 +2,16 @@ import com.ftcteams.behaviortrees.DebugTree; import com.ftcteams.behaviortrees.Node; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.util.ElapsedTime; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees.PreloadAuto; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees.TwoSpecimenAutoTree; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees.FourSpecimenTree; //@Autonomous -public class TwoSpecimenAuto extends QQOpMode{ +public class OldAuto extends QQOpMode{ boolean clawWasClosed; boolean test; boolean done; - Node root = TwoSpecimenAutoTree.root(); + Node root = FourSpecimenTree.root(); DebugTree debugTree = new DebugTree(); ElapsedTime moveTimer = new ElapsedTime(); double INIT_MOVE_TIME_SECONDS = 1; @@ -24,6 +22,7 @@ public void init(){ moveTimer.reset(); robot.scoreArm.goToInit(); robot.scoringClaw.close(); + robot.intakeArm.goToDropPos(); } @Override @@ -38,7 +37,8 @@ public void init_loop() { } public void start(){ - robot.otos.setOtosPosition(7,-61.5,0); + super.start(); + robot.otos.setOtosPosition(10,32,0); } public void loop() { @@ -48,6 +48,9 @@ public void loop() { robot.scoreArm.goToPlace(); test = true; } + if(robot.scoreArm.isStalling()){ + robot.scoringClaw.open(); + } if (robot.scoringClaw.isClawOpen()){ test = false; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PreloadAutoBehaviorTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PreloadAutoBehaviorTree.java deleted file mode 100644 index c03766f..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PreloadAutoBehaviorTree.java +++ /dev/null @@ -1,61 +0,0 @@ -package org.firstinspires.ftc.teamcode.ftc16072.OpModes; - -import com.ftcteams.behaviortrees.DebugTree; -import com.ftcteams.behaviortrees.Node; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.util.ElapsedTime; - -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees.PreloadAuto; - -@Autonomous -public class PreloadAutoBehaviorTree extends QQOpMode{ - boolean clawWasClosed; - boolean done; - Node root = PreloadAuto.root(); - DebugTree debugTree = new DebugTree(); - ElapsedTime moveTimer = new ElapsedTime(); - double INIT_MOVE_TIME_SECONDS = 1; - double INIT_MOVE_SPEED = -0.2; - @Override - public void init(){ - super.init(); - moveTimer.reset(); - robot.scoreArm.goToInit(); - robot.scoringClaw.close(); - } - - @Override - public void init_loop() { - if (moveTimer.seconds() < INIT_MOVE_TIME_SECONDS){ - robot.mecanumDrive.move(INIT_MOVE_SPEED,0,0); - }else { - robot.mecanumDrive.stop(); - robot.controlHub.resetGyro(); - } - robot.scoreArm.update(telemetry); - - } - public void start(){ - robot.otos.setOtosPosition(7,-61.5,0); - } - - public void loop() { - super.loop(); - if (robot.scoringClaw.isClawClosed() && !clawWasClosed) { - robot.intakeClaw.open(); - robot.scoreArm.goToPlace(); - } - if (robot.scoringClaw.isScoreSwitchPressed()){ - robot.scoreArm.setNotScoring(); - } - if(!done){ - Node.State state = root.tick(debugTree, this); - telemetry.addData("BT",debugTree); - if(state == Node.State.SUCCESS){ - done = true; - } - } - clawWasClosed = robot.scoringClaw.isClawClosed(); - } -} - diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PushintoNetandAscendAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PushintoNetandAscendAuto.java index 23d421c..8feadba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PushintoNetandAscendAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PushintoNetandAscendAuto.java @@ -2,7 +2,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -@Autonomous +//@Autonomous public class PushintoNetandAscendAuto extends QQOpMode{ private double step = 0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/QQOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/QQOpMode.java index 24caa5f..f3d5c1c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/QQOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/QQOpMode.java @@ -15,12 +15,10 @@ public abstract class QQOpMode extends OpMode { @Override public void init() { telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - nav = new Navigation(robot, telemetry); robot.init(hardwareMap); robot.controlHub.resetGyro(); - //robot.claw.close(); - //robot.arm.goToDrive(); isAllianceRed = robot.scoringClaw.isColorRed(); + nav = new Navigation(robot, telemetry, isAllianceRed); if(isAllianceRed) { telemetry.addData("Alliance", "RED"); robot.intakeClaw.setTargetColorRed(); @@ -39,6 +37,11 @@ public void init_loop(){ } } + @Override + public void start(){ + robot.limelight.start(); + } + @Override public void loop(){ robot.update(telemetry); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/SpecimenCycleAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/SpecimenCycleAuto.java index cda5445..46030ef 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/SpecimenCycleAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/SpecimenCycleAuto.java @@ -6,9 +6,8 @@ import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees.SpecimenCycleAutoTree; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees.TwoSpecimenAutoTree; -@Autonomous +//@Autonomous public class SpecimenCycleAuto extends QQOpMode{ boolean clawWasClosed; boolean test; @@ -40,6 +39,7 @@ public void init_loop() { } public void start(){ + super.start(); robot.otos.setOtosPosition(7,-61.5,0); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java index 07d7550..064cb2f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java @@ -3,6 +3,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Limelight; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.ScoringClaw; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.ControlHub; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.IntakeArm; @@ -23,16 +24,13 @@ public class Robot { public OpticalTrackingOdometrySensor otos; public ScoringClaw scoringClaw; public IntakeArm intakeArm; - //public DoubleReverse4Bar doubleReverse4Bar; - // public Slides slides; public IntakeClaw intakeClaw; - boolean isRedAlliance; - public IntakeSlides intakeSlides; public ScoreArm scoreArm; + public Limelight limelight; boolean driveOnly; @@ -46,11 +44,9 @@ public Robot() { scoringClaw = new ScoringClaw(); intakeSlides = new IntakeSlides(); intakeArm = new IntakeArm(); - //doubleReverse4Bar = new DoubleReverse4Bar(); - //slides = new Slides(); - scoreArm = new ScoreArm(); intakeClaw = new IntakeClaw(); + limelight = new Limelight(); mechanisms = Arrays.asList( controlHub, @@ -60,6 +56,7 @@ public Robot() { intakeArm, intakeSlides, scoreArm, + limelight, intakeClaw); } @@ -74,11 +71,6 @@ public void update(Telemetry telemetry){ } } - public void setAlliancecolor(){ - if (scoringClaw.isColorRed()){ - isRedAlliance = true; - }isRedAlliance = false; - } public void makeDriveOnly(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Tests/TestLimelight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Tests/TestLimelight.java new file mode 100644 index 0000000..9363a6a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Tests/TestLimelight.java @@ -0,0 +1,18 @@ +package org.firstinspires.ftc.teamcode.ftc16072.Tests; + +import com.qualcomm.hardware.limelightvision.Limelight3A; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +public class TestLimelight extends QQTest{ +Limelight3A limelight; + + public TestLimelight(String name, Limelight3A limelight){ + super(name); + this.limelight = limelight; + } + @Override + public void run(boolean on, Telemetry telemetry) { + telemetry.addData("Attached", limelight.isConnected()); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/Navigation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/Navigation.java index 171b929..15a0673 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/Navigation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/Navigation.java @@ -1,10 +1,13 @@ package org.firstinspires.ftc.teamcode.ftc16072.Util; +import android.util.Log; + import com.acmerobotics.dashboard.config.Config; import com.qualcomm.hardware.sparkfun.SparkFunOTOS; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.ftc16072.Robot; @Config public class Navigation { @@ -16,23 +19,28 @@ public class Navigation { public static double TRANSLATIONAL_KP = 0.03; public static double TRANSLATIONAL_KI = 0.0; - public static double TRANSLATIONAL_KD = 0.000; + public static double TRANSLATIONAL_KD = 0.0; public static double TRANSLATIONAL_KF = 0; - public static double TRANSLATIONAL_TOLERANCE_THRESHOLD = 1; + public static double TRANSLATIONAL_TOLERANCE_THRESHOLD = 3; + public static double TRANSLATE_ABSOLUTE_MIN = 0.1; public static double ROTATIONAL_KP = 0.01; public static double ROTATIONAL_KI = 0.000; public static double ROTATIONAL_KD = 0.00; public static double ROTATIONAL_KF = 0; public static double ROTATIONAL_TOLERANCE_THRESHOLD = 3; + private boolean isRed; PIDFController PIDx, PIDy, PIDh; + boolean isZeroed; + double lastDesiredX, lastDesiredY, lastDesiredH; Telemetry telemetry; - public Navigation(Robot robot, Telemetry telemetry){ + public Navigation(Robot robot, Telemetry telemetry, boolean isRed){ this.robot = robot; + this.isRed = isRed; PIDx = new PIDFController(TRANSLATIONAL_KP,TRANSLATIONAL_KI,TRANSLATIONAL_KD,TRANSLATIONAL_KF, MAX_TRANSLATE, MIN_TRANSLATE); PIDy = new PIDFController(TRANSLATIONAL_KP,TRANSLATIONAL_KI,TRANSLATIONAL_KD,TRANSLATIONAL_KF,MAX_TRANSLATE,MIN_TRANSLATE); PIDh = new PIDFController(ROTATIONAL_KP,ROTATIONAL_KI,ROTATIONAL_KD,ROTATIONAL_KF, MAX_ROTATE, MIN_ROTATE); @@ -43,13 +51,13 @@ public void driveFieldRelative(double forwardSpeed, double strafeRightSpeed, dou //convert to polar double theta = Math.atan2(forwardSpeed, strafeRightSpeed); double r = Math.hypot(forwardSpeed, strafeRightSpeed); - + theta = AngleUnit.normalizeRadians(theta - robotAngle); - + //convert to cartesian double newForwardSpeed = r * Math.sin(theta); double newStrafeRightSpeed = r * Math.cos(theta); - + robot.mecanumDrive.move(newForwardSpeed,newStrafeRightSpeed,rotateCWSpeed); } @@ -59,14 +67,29 @@ private boolean notWithinTolerance(double desired, double current, double tolera return !(Math.abs(error) < tolerance); } - public boolean driveToPositionIN(double desiredX,double desiredY,double desiredHeading){ + public boolean driveToPositionIN(double desiredX,double desiredY,double desiredHeading, double xTolerance, double yTolerance, double headingTolerance){ double forwardSpeed; - double strafeLeftSpeed; + double strafeRightSpeed; double rotateCCWSpeed; PIDx.updateConstants(TRANSLATIONAL_KP,TRANSLATIONAL_KI,TRANSLATIONAL_KD,TRANSLATIONAL_KF,MAX_TRANSLATE,MIN_TRANSLATE); PIDx.updateConstants(TRANSLATIONAL_KP,TRANSLATIONAL_KI,TRANSLATIONAL_KD,TRANSLATIONAL_KF,MAX_TRANSLATE,MIN_TRANSLATE); PIDy.updateConstants(TRANSLATIONAL_KP,TRANSLATIONAL_KI,TRANSLATIONAL_KD,TRANSLATIONAL_KF,MAX_TRANSLATE,MIN_TRANSLATE); PIDy.updateConstants(TRANSLATIONAL_KP,TRANSLATIONAL_KI,TRANSLATIONAL_KD,TRANSLATIONAL_KF,MAX_TRANSLATE,MIN_TRANSLATE); + if (!isZeroed){ + if(robot.limelight.isAprilTagSeen()){ + isZeroed = true; + double xPosition = robot.limelight.getRobotPositionX(DistanceUnit.INCH); + double yPosition = robot.limelight.getRobotPositionY(DistanceUnit.INCH); + if(isRed){ + xPosition = -xPosition; + yPosition = -yPosition; + } + robot.otos.setOtosPosition( + xPosition, + yPosition, + robot.controlHub.getYaw(AngleUnit.DEGREES)); + } + } if((desiredX != lastDesiredX) || (desiredY != lastDesiredY) || (desiredHeading != lastDesiredH)){ lastDesiredX = desiredX; lastDesiredY = desiredY; @@ -75,31 +98,50 @@ public boolean driveToPositionIN(double desiredX,double desiredY,double desiredH PIDy.reset(); PIDh.reset(); } - SparkFunOTOS.Pose2D currentPosition = robot.otos.getOtosPosition(); - if(notWithinTolerance(desiredX,currentPosition.x,TRANSLATIONAL_TOLERANCE_THRESHOLD)){ - forwardSpeed = PIDx.calculate(desiredX, currentPosition.x); - }else{forwardSpeed = 0;} - if(notWithinTolerance(desiredY,currentPosition.y,TRANSLATIONAL_TOLERANCE_THRESHOLD)){ - strafeLeftSpeed = PIDy.calculate(desiredY, currentPosition.y); - }else {strafeLeftSpeed = 0;} - if (notWithinTolerance(desiredHeading, currentPosition.h,ROTATIONAL_TOLERANCE_THRESHOLD)) { - rotateCCWSpeed = PIDh.calculate(desiredHeading, currentPosition.h); + double currentPositionX = robot.otos.getOtosPosition().x; + double currentPositionY = robot.otos.getOtosPosition().y; + double currentPositionH = robot.controlHub.getYaw(AngleUnit.DEGREES); + if(notWithinTolerance(desiredX,currentPositionX,xTolerance)){ + strafeRightSpeed = PIDx.calculate(desiredX, currentPositionX); + strafeRightSpeed = Math.signum(strafeRightSpeed)* Math.max(Math.abs(strafeRightSpeed),TRANSLATE_ABSOLUTE_MIN); + }else {strafeRightSpeed = 0;} + if(notWithinTolerance(desiredY,currentPositionY,yTolerance)){ + forwardSpeed = PIDy.calculate(desiredY, currentPositionY); + forwardSpeed = Math.signum(forwardSpeed)* Math.max(Math.abs(forwardSpeed),TRANSLATE_ABSOLUTE_MIN); + }else {forwardSpeed = 0;} + if (notWithinTolerance(desiredHeading, currentPositionH,headingTolerance)) { + rotateCCWSpeed = PIDh.calculate(desiredHeading, currentPositionH); }else {rotateCCWSpeed = 0;} - telemetry.addData("Current X", currentPosition.x); + telemetry.addData("Current X", currentPositionX); telemetry.addData("Desired X", desiredX); telemetry.addData("Desired Y", desiredY); - telemetry.addData("Current Y", currentPosition.y); + telemetry.addData("Current Y", currentPositionY); telemetry.addData("Desired Heading", desiredHeading); - telemetry.addData("Current Heading", currentPosition.h); + telemetry.addData("Current Heading", currentPositionH); - telemetry.addData("strafe right speed",-strafeLeftSpeed); - telemetry.addData("foward speed",forwardSpeed); + telemetry.addData("strafe right speed",strafeRightSpeed); + telemetry.addData("forward speed",forwardSpeed); telemetry.addData("rotate CW Speed", -rotateCCWSpeed); + Log.d("QQ", "X (desired, current, speed):" + desiredX + " " + currentPositionX + " " + strafeRightSpeed); + Log.d("QQ", "Y (desired, current, speed):" + desiredY + " " + currentPositionY + " " + forwardSpeed); + - driveFieldRelative(forwardSpeed,-strafeLeftSpeed,-rotateCCWSpeed); - return !(notWithinTolerance(desiredX,currentPosition.x,TRANSLATIONAL_TOLERANCE_THRESHOLD)|| - notWithinTolerance(desiredY,currentPosition.y,TRANSLATIONAL_TOLERANCE_THRESHOLD)|| - notWithinTolerance(desiredHeading, currentPosition.h,ROTATIONAL_TOLERANCE_THRESHOLD)); + driveFieldRelative(forwardSpeed,strafeRightSpeed,-rotateCCWSpeed); + boolean isFinished = !(notWithinTolerance(desiredX,currentPositionX,xTolerance)|| + notWithinTolerance(desiredY,currentPositionY,yTolerance)|| + notWithinTolerance(desiredHeading,currentPositionH,headingTolerance)); + if(isFinished){ + isZeroed = false; + }return isFinished; + } + public boolean driveToPositionIN(double desiredX,double desiredY,double desiredHeading, double translationTolerance, double headingTolerance){ + return driveToPositionIN(desiredX,desiredY,desiredHeading,translationTolerance,translationTolerance,headingTolerance); + } + public boolean driveToPositionIN(double desiredX,double desiredY,double desiredHeading, double translationTolerance){ + return driveToPositionIN(desiredX,desiredY,desiredHeading,translationTolerance,translationTolerance,ROTATIONAL_TOLERANCE_THRESHOLD); + } + public boolean driveToPositionIN(double desiredX,double desiredY,double desiredHeading){ + return driveToPositionIN(desiredX,desiredY,desiredHeading,TRANSLATIONAL_TOLERANCE_THRESHOLD,TRANSLATIONAL_TOLERANCE_THRESHOLD,ROTATIONAL_TOLERANCE_THRESHOLD); } } \ No newline at end of file diff --git a/TeamCode/src/main/res/xml/specimen_bot.xml b/TeamCode/src/main/res/xml/specimen_bot.xml index c4ba873..a41fa3c 100644 --- a/TeamCode/src/main/res/xml/specimen_bot.xml +++ b/TeamCode/src/main/res/xml/specimen_bot.xml @@ -11,7 +11,7 @@ - + @@ -28,5 +28,6 @@ +