Merge pull request #36 from ftc16072/increase-score-power

Added behavior trees to auto and coded a new preload place auto

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToChamber.java

@@ -0,0 +1,36 @@
+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 DriveToChamber extends QQTimeoutNode {
+    public static final double FORWARD_SPEED = 0.3;
+    State lastStatus = State.RUNNING;
+
+    public DriveToChamber(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,0,0);
+            if (opMode.robot.scoreArm.isChamberContacted()){
+                opMode.robot.mecanumDrive.stop();
+                lastStatus = State.SUCCESS;
+                return State.SUCCESS;
+
+            }
+            return State.RUNNING;
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Park.java

@@ -0,0 +1,25 @@
+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 Park extends QQTimeoutNode {
+    public Park(double seconds) {
+        super(seconds);
+    }
+
+    @Override
+    public State tick(DebugTree debug, QQOpMode opMode) {
+        boolean isDoneDriving = opMode.nav.driveToPositionIN(14,-109.5,0);
+        opMode.telemetry.addData("location",opMode.robot.otos.getOtosPosition());
+        if (isDoneDriving) {
+            return State.SUCCESS;
+        }
+        if (hasTimedOut()) {
+            return State.FAILURE;
+        }
+        return State.RUNNING;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/WaitForClawOpen.java

@@ -0,0 +1,23 @@
+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 WaitForClawOpen extends QQTimeoutNode {
+    public WaitForClawOpen(double seconds) {
+        super(seconds);
+    }
+
+    @Override
+    public State tick(DebugTree debug, QQOpMode opMode) {
+        if (hasTimedOut()){
+            opMode.robot.scoreArm.setNotScoring();
+            return State.FAILURE;
+        }else if (opMode.robot.scoringClaw.isClawOpen()){
+            return State.SUCCESS;
+        }
+        return State.RUNNING;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/WaitForScore.java

@@ -0,0 +1,24 @@
+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 WaitForScore extends QQTimeoutNode {
+    public WaitForScore(double seconds) {
+        super(seconds);
+    }
+
+    @Override
+    public State tick(DebugTree debug, QQOpMode opMode) {
+        if (hasTimedOut()){
+            opMode.robot.scoreArm.setNotScoring();
+            opMode.robot.scoringClaw.open();
+            return State.FAILURE;
+        }else if (opMode.robot.scoringClaw.isScoreSwitchPressed()){
+            return State.SUCCESS;
+        }
+        return State.RUNNING;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/QQNode.java

@@ -0,0 +1,14 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees;
+
+import com.ftcteams.behaviortrees.DebugTree;
+import com.ftcteams.behaviortrees.Node;
+
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
+
+abstract public class QQNode extends Node {
+    @Override
+    public State tick(DebugTree debug, Object obj) {
+        return tick(debug, (QQOpMode)obj);
+    }
+    abstract public State tick(DebugTree debug, QQOpMode opMode);
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/QQTimeoutNode.java

@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees;
+
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+abstract public class QQTimeoutNode extends QQNode {
+    ElapsedTime timer = new ElapsedTime();
+    public double seconds;
+    boolean isTimerStarted;
+
+    public QQTimeoutNode(double seconds){
+        super();
+        this.seconds = seconds;
+    }
+    public boolean hasTimedOut(){
+        if (!isTimerStarted){
+            timer.reset();
+            isTimerStarted = true;
+        }
+        if (timer.seconds() >= seconds){
+            return true;
+        }
+        return false;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/PreloadAuto.java

@@ -0,0 +1,35 @@
+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.Sequence;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveToChamber;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.WaitForClawOpen;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.WaitForScore;
+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 Park(TIMEOUT_SECONDS)),
+                new Park(TIMEOUT_SECONDS));
+    }
+}
+
+/* TREE
+?
+|  ->
+|  |  [DriveToChamber]
+|  |  [Score]
+|  |  [Park]
+|  [Park]
+ */

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/MecanumDrive.java

@@ -78,6 +78,9 @@ public void move (double forwardSpeed, double strafeRightSpeed, double turnClock
         setPowers(frontLeftPower,frontRightPower,backRightPower,backLeftPower);
 
     }
+    public void stop(){
+        move(0,0,0);
+    }
     private void setPowers(double frontLeftPower, double frontRightPower,double backRightPower, double backLeftPower){
         double maxSpeed = 1.0;
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java

@@ -19,19 +19,18 @@
 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;
     DcMotor leftMotor;
     DcMotor rightMotor;
     TouchSensor limitSwitch;
     TouchSensor rightChamberContact;
     TouchSensor leftChamberContact;
-    public static final double SCORE_POWER = -0.8;
     public static double kP = 0.00159;
     public static double kI = 0.0;
     public static double kD = 0;
     public static double kF = 0;
-    public static double max =  0.8;
-    public static double min = -0.8;
-
+    public static double max =  1.0;
+    public static double min = -1.0;
     boolean chamberContacted;
     protected int currentPos;
     public int desiredPos;
@@ -89,9 +88,19 @@ public void update(Telemetry telemetry){
                 desiredPos = 0;
             }
         }
+        if (limitSwitch.isPressed()){
+            isScoring = false;
+        }else if (chamberContacted){
+            isScoring = true;
+            goToScoring();
+        }
         currentPos = (leftMotor.getCurrentPosition() + rightMotor.getCurrentPosition())/2;//average left and right speeds
+
         double motorPower = pidfController.calculate(desiredPos,currentPos);
         this.motorPower = motorPower;
+        if (isScoring){
+            motorPower = SCORE_POWER;
+        }
         leftMotor.setPower(motorPower);
         rightMotor.setPower(motorPower);
         if (isScoring){

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoringClaw.java

@@ -66,6 +66,9 @@ public boolean isClawClosed() {
 
         return false;
     }
+    public boolean isScoreSwitchPressed(){
+        return !scoreSwitch.isPressed();
+    }
 
 
     public boolean isBlockGrabbable() {
@@ -86,7 +89,6 @@ public void close() {
         clawServo.setPosition(CLAW_CLOSE_POSITION);
         closedTimer.reset();
     }
-    public boolean isScoreSwitchPressed(){ return !scoreSwitch.isPressed();}
 
     @Override
     public void update(Telemetry telemetry) {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/FieldRelativeDrive.java

@@ -8,12 +8,13 @@ public class FieldRelativeDrive extends QQOpMode{
 
     @Override
     public void init(){
-        robot.makeDriveOnly();
         super.init();
     }
 
     @Override
     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);
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NavigationPIDTuner.java

@@ -0,0 +1,45 @@
+package org.firstinspires.ftc.teamcode.ftc16072.OpModes;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+@TeleOp
+public class NavigationPIDTuner extends QQOpMode{
+    private final int autodelay = 0;
+
+    ElapsedTime elapsedTime = new ElapsedTime();
+    private double step = 0;
+    boolean isPlacing;
+    boolean chamberContactWasPressed;
+    int contactLostPos;
+    boolean clawWasClosed;
+    public void init(){
+        super.init();
+        robot.otos.setOtosPosition(7,-61.5,0);
+        robot.scoringClaw.close();
+    }
+    public void start(){
+        elapsedTime.reset();
+    }
+    public void loop(){
+        super.loop();
+
+        if(step == 0){
+            boolean isDoneDriving = nav.driveToPositionIN(7.5, -37.5,0);
+            if (isDoneDriving){
+                step = 1;
+            }
+        }else if(step == 1){
+            boolean isDoneDriving = nav.driveToPositionIN(7.5,-85.5,0);
+            if (isDoneDriving){
+                step = 0;
+            }
+        }
+        chamberContactWasPressed = robot.scoreArm.isChamberContacted();
+        clawWasClosed = robot.scoringClaw.isClawClosed();
+    }
+}
+
+
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PreloadAutoBehaviorTree.java

@@ -0,0 +1,62 @@
+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.goToScoring();
+        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();
+    }
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/QQOpMode.java

@@ -9,13 +9,13 @@
 
 public abstract class QQOpMode extends OpMode {
     public Robot robot = new Robot();
-    Navigation nav = new Navigation(robot, telemetry);
+    public Navigation nav;
 
     @Override
     public void init() {
-        robot.init(hardwareMap);
-        robot.controlHub.resetGyro();
         telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        nav = new Navigation(robot, telemetry);
+        robot.init(hardwareMap);
         robot.controlHub.resetGyro();
         //robot.claw.close();
         //robot.arm.goToDrive();