Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Teleopfixes #65

Merged
merged 7 commits into from
Feb 17, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view

This file was deleted.

This file was deleted.

This file was deleted.

Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@ public class UpdateIntake extends Node {
@Override
public State tick(QQOpMode opmode) {
if (opmode.gamepad1.right_trigger > TRIGGER_THRESHOLD){
opmode.robot.intake.intake();
opmode.robot.intake.intake(opmode.gamepad1.right_trigger);
}
else if (opmode.gamepad1.x){
opmode.robot.intake.eject();
Expand Down

This file was deleted.

This file was deleted.

Original file line number Diff line number Diff line change
@@ -1,39 +1,11 @@
package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees;

import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.AddTelemetry;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ClampOnPixel;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveFieldRelative;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MakeFastDrive;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MakeNormalDrive;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MakeSlowDrive;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MoveArmAndLift;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MoveLiftToIntakePosition;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MoveLiftToPixelGrabPosition;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.PlacePixels;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.PlacePixels2;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ResetGyro;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SetLiftPosition;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SpinInIntakeMotor;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SpinOutIntakeMotor;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.StopIntakeMotor;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.UpdateArmAndLift;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.UpdateClimber;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.AreNotSlidesExtended;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.AreSlidesExtended;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.Has1Pixel;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.Has1or2Pixels;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.Has2Pixels;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.HasLessThan2Pixels;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfEjectButtonPressed;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfIntakeButtonPressed;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfLeftTriggerPressed;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfLiftAtBottom;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfLiftToPixelGrabPosButtonPressed;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IsControllerDriving;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Failover;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.UpdateIntake;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Parallel;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Sequence;

/*
http://behaviortrees.ftcteams.com/
Expand Down Expand Up @@ -94,23 +66,9 @@
public class PushbotTeleop {
public static Node root(){
return new Parallel(5,
new Failover(
new Sequence(
new IfIntakeButtonPressed(),
new SpinInIntakeMotor()
),
new Sequence(
new IfEjectButtonPressed(), // might want to consider removing this and automatically ejecting
new SpinOutIntakeMotor()

),
new StopIntakeMotor()

),
new UpdateIntake(),
new ResetGyro(),

new DriveFieldRelative(),

new UpdateClimber()

);
Expand Down
Original file line number Diff line number Diff line change
@@ -1,37 +1,20 @@
package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees;

import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.AddTelemetry;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ClampOnPixel;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveFieldRelative;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DroneRelease;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MakeFastDrive;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MakeNormalDrive;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MakeSlowDrive;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MoveArmAndLift;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MoveLiftToIntakePosition;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MoveLiftToPixelGrabPosition;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.PlacePixels;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.PlacePixels2;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ResetGyro;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.RumbleGamepad;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SetLiftPosition;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SpinInIntakeMotor;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SpinOutIntakeMotor;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.StopIntakeMotor;

import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.UpdateArmAndLift;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.UpdateClimber;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.AreNotSlidesExtended;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.UpdateIntake;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.AreSlidesExtended;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.Has1Pixel;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.Has1or2Pixels;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.Has2Pixels;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.HasLessThan2Pixels;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfEjectButtonPressed;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfIntakeButtonPressed;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfLeftTriggerPressed;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfLiftAtBottom;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfLiftToPixelGrabPosButtonPressed;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IsBackboardInRange;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IsControllerDriving;
import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.ifDroneReleaseButtonPressed;
Expand Down Expand Up @@ -175,17 +158,7 @@ public static Node root(){
new StopIntakeMotor()

*/
new Failover(
new Sequence(
new IfEjectButtonPressed(),
new SpinOutIntakeMotor()
),
new Sequence(
new IfIntakeButtonPressed(),
new SpinInIntakeMotor()
),
new StopIntakeMotor()
),
new UpdateIntake(),

/*new Sequence(
new SetLiftPosition()
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -29,8 +29,8 @@ public List<QQtest> getTests() {
new TestMotor("Eject", EJECT_SPEED, intakeMotor));

}
public void intake(){
intakeMotor.setPower(INTAKE_SPEED);
public void intake(double amount){
intakeMotor.setPower(INTAKE_SPEED * amount);
}
public void eject(){
intakeMotor.setPower(EJECT_SPEED);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@

import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.ftc16072.QQTest.QQtest;
import org.firstinspires.ftc.teamcode.ftc16072.QQTest.TestMotor;
import org.firstinspires.ftc.teamcode.ftc16072.QQTest.TestTwoMotor;

import java.util.Arrays;
Expand Down Expand Up @@ -38,7 +39,7 @@ public enum LiftPositions{

private LiftPositions manipulatorPosition;
private static final int LIFT_POSITION_SAFETY_BOTTOM = -50;
private static final int LIFT_POSITION_SAFETY_TOP = 343455; //TODO need to fix
private static final int LIFT_POSITION_SAFETY_TOP = 2600; //TODO: find actual

private static final int PIXEL_GRAB_POSITION=0;

Expand All @@ -58,11 +59,9 @@ public enum LiftPositions{
private double desiredPosition;
private double sumOfErrors;
private double lastError;
public static double K_P = 0.007;
public static double K_I = 0.0001;
public static double K_D = 0.2;
public double motorPower;

public static double K_P = 0.002;
public static double K_I = 0;
public static double K_D = 0;


@Override
Expand All @@ -84,6 +83,13 @@ public void init(HardwareMap hwMap) {

}
private void setDesiredPosition(double newPosition){
if(newPosition > LIFT_POSITION_SAFETY_TOP){
newPosition = LIFT_POSITION_SAFETY_TOP;
}
if(newPosition < LIFT_POSITION_SAFETY_BOTTOM){
newPosition = LIFT_POSITION_SAFETY_BOTTOM;
}

desiredPosition = newPosition;
sumOfErrors = 0;
lastError = 0;
Expand All @@ -94,16 +100,27 @@ public boolean areSlidesExtendedPastBoundary(){

}
public void update(Telemetry telemetry){

double motorPower;
double error;
error = getDesiredPosition() - currentPosition();
telemetry.addData("Desired Pos", getDesiredPosition());
telemetry.addData("Curr Pos", currentPosition());

sumOfErrors = sumOfErrors + error;

//motorPower =K_P * error + K_I * sumOfErrors + K_D * (error - lastError);
motorPower = K_P *error;
motorPower =K_P * error + K_I * sumOfErrors + K_D * (error - lastError);

lastError = error;
telemetry.addData("error", error);

/*
if (Math.abs(motorPower) < 0.1){
motorPower = 0;
}
*/

telemetry.addData("lift power: ",motorPower);

rightLiftMotor.setPower(motorPower);
leftLiftMotor.setPower(motorPower);
}
Expand All @@ -112,24 +129,17 @@ public void update(Telemetry telemetry){
public List<QQtest> getTests() {
return Arrays.asList(
new TestTwoMotor("lift", leftLiftMotor, rightLiftMotor, 0.5),
new TestTwoMotor("downLift", leftLiftMotor, rightLiftMotor, -0.5)
new TestTwoMotor("downLift", leftLiftMotor, rightLiftMotor, -0.5),
new TestMotor("right lift motor", 0.5, rightLiftMotor),
new TestMotor("left lift motor", 0.5, leftLiftMotor)
);
}

public void manualLiftUp(){
setDesiredPosition(currentPosition() + MANUAL_CHANGE);
if(getDesiredPosition() > LIFT_POSITION_SAFETY_TOP){
desiredPosition = LIFT_POSITION_SAFETY_TOP;
}
}public void manualLiftDown(){
setDesiredPosition(currentPosition()-MANUAL_CHANGE);
if(getDesiredPosition() < LIFT_POSITION_SAFETY_BOTTOM){
setDesiredPosition(LIFT_POSITION_SAFETY_BOTTOM);

}
setDesiredPosition(getDesiredPosition() + MANUAL_CHANGE);
}
public double getPower(){
return motorPower;
public void manualLiftDown(){
setDesiredPosition(getDesiredPosition() - MANUAL_CHANGE);
}
public double getDesiredPosition(){
return desiredPosition;
Expand Down
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.ftc16072.OpModes;

import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;

import org.firstinspires.ftc.robotcore.external.Telemetry;
Expand All @@ -22,6 +24,8 @@ abstract public class QQOpMode extends OpMode {
* every opmode initializes robot with hardware map
*/
public void init(){
FtcDashboard dashboard = FtcDashboard.getInstance();
telemetry = new MultipleTelemetry(telemetry, dashboard.getTelemetry());
robot.init(hardwareMap);
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -7,15 +7,6 @@
@TeleOp
@Config
public class TestingRobotTeleOp extends QQOpMode {
FtcDashboard dashboard;

public void init(){
robot.init(hardwareMap);
dashboard = FtcDashboard.getInstance();
telemetry = dashboard.getTelemetry();


}
@Override
public void loop() {
if (gamepad1.a){
Expand Down Expand Up @@ -53,7 +44,6 @@ else if(gamepad2.dpad_down){
robot.climber.stop();
}

telemetry.addData("lift motor power ",robot.lift.getPower());
telemetry.addData("lift desired Position", robot.lift.getDesiredPosition());
telemetry.addData("lift current position", robot.lift.currentPosition());
robot.lift.update(telemetry);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,8 @@ public boolean atPosition(Lift.LiftPositions liftPosition){
}
public void manualLiftUp(){
lift.manualLiftUp();
}public void manualLiftDown(){
}
public void manualLiftDown(){
lift.manualLiftDown();
}

Expand Down
Loading