eaglesfe
/
robotcode
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Drive still needs work

This commit is contained in:
Scott Barnes 2023-11-09 20:05:31 -06:00
parent 9be27bc6b8
commit bd0f1305ce
9 changed files with 204 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
TeamCode/build.gradle
View File

@ -30,4 +30,5 @@ dependencies {
implementation 'org.apache.commons:commons-math3:3.6.1'
implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7'
implementation 'com.acmerobotics.roadrunner:core:0.5.6'
implementation 'org.ftclib.ftclib:core:2.1.1' // core
}

98
TeamCode/src/main/java/opmodes/MainTeleOp.java
View File

@ -1,9 +1,6 @@
package opmodes;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_ARM_DELTA;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_LIFT_DELTA;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_SCREW_DELTA;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_X_DELTA;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PICKUP_ARM_MAX;
import com.acmerobotics.dashboard.FtcDashboard;
@ -19,18 +16,19 @@ public class MainTeleOp extends OpMode {
private double clawArmPosition = 0;
private boolean screwArmPos = false;
private boolean previousScrewArmToggle = false;
private boolean previousScrewDeposit = false;
private boolean previousScrewReset = false;
private boolean previousSlideUp = false;
private boolean previousSlideDown = false;
private double currentScrewPosition = 1f;
private boolean previousRobotLiftReset = false;
private boolean previousRobotLiftExtend = false;
private boolean liftArmShouldBeUp = false;
private boolean screwArmIsMoving = false;
@Override
public void init() {
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
this.clawArmPosition = PICKUP_ARM_MAX;
this.robot = new Robot(this.hardwareMap);
this.robot = new Robot(this.hardwareMap, telemetry);
telemetry.addData("Status", "Initialized");
}
@ -50,12 +48,13 @@ public class MainTeleOp extends OpMode {
boolean clawUp = gamepad2.y; // Y
boolean clawDown = gamepad2.a; // A
boolean raiseRobotLift = gamepad2.right_trigger > 0.25; // RT
boolean liftRobot = gamepad2.right_bumper; // RB
boolean robotLiftRotation = gamepad2.right_trigger > 0.05; // RT
boolean robotLiftExtend = gamepad2.right_trigger > 0.5; // RT
boolean robotLiftReset = gamepad2.right_stick_button;
boolean screwArmToggle = gamepad2.x; // X
boolean screwDeposit = gamepad2.left_trigger > 0.25; // LT
boolean screwReset = gamepad2.left_bumper; // LB
boolean screwIntake = gamepad2.left_bumper; // LB
boolean slideUp = gamepad2.left_stick_y < -0.25; // Left Y Axis Joystick
boolean slideDown = gamepad2.left_stick_y > 0.25; // Left Y Axis Joystick
boolean gantryLeft = gamepad2.dpad_left; // dpad left
@ -64,56 +63,81 @@ public class MainTeleOp extends OpMode {
// Claw
if (openClaw) {
this.robot.getClaw().open();
} else if (!clawUp && !clawDown){
this.screwArmIsMoving = false;
} else if (!clawUp && !clawDown && !this.screwArmIsMoving){
this.robot.getClaw().close();
}
if (clawUp) {
this.screwArmIsMoving = false;
this.clawArmPosition = Math.min(1, this.clawArmPosition + CLAW_ARM_DELTA);
this.robot.getClaw().setArmPosition(clawArmPosition);
} else if (clawDown) {
this.screwArmIsMoving = false;
this.robot.getClaw().open();
this.clawArmPosition = Math.max(0, this.clawArmPosition - CLAW_ARM_DELTA);
this.robot.getClaw().setArmPosition(clawArmPosition);
}
/*
// Gantry
if (!previousScrewArmToggle && screwArmToggle) {
this.screwArmIsMoving = true;
if (screwArmPos) {
this.robot.getGantry().armOut();
} else {
this.robot.getGantry().armIn();
} else {
this.robot.getClaw().open();
this.robot.getGantry().armOut();
}
this.robot.getClaw().open();
screwArmPos = !screwArmPos;
}
if (!previousScrewDeposit && screwDeposit) {
this.currentScrewPosition += GANTRY_SCREW_DELTA;
this.robot.getGantry().setScrew(currentScrewPosition);
} else if (!previousScrewReset && screwReset) {
this.robot.getGantry().resetScrew();
}
if ((!previousSlideUp && slideUp) || (!previousSlideDown && slideDown)) {
int currentPosition = this.robot.getGantry().getSlidePosition();
this.robot.getGantry().setTarget(currentPosition + GANTRY_LIFT_DELTA);
}
if (gantryLeft) {
this.robot.getGantry().setX(this.robot.getGantry().getX() + GANTRY_X_DELTA);
} else if (gantryRight) {
this.robot.getGantry().setX(this.robot.getGantry().getX() - GANTRY_X_DELTA);
if (screwDeposit) {
this.robot.getGantry().deposit();
} else if (screwIntake) {
this.robot.getGantry().intake();
} else {
this.robot.getGantry().stop();
}
// if ((!previousSlideUp && slideUp) || (!previousSlideDown && slideDown)) {
// int currentPosition = this.robot.getGantry().getSlidePosition();
// this.robot.getGantry().setTarget(currentPosition + GANTRY_LIFT_DELTA);
// }
//
// if (gantryLeft) {
// this.robot.getGantry().setX(this.robot.getGantry().getX() + GANTRY_X_DELTA);
// } else if (gantryRight) {
// this.robot.getGantry().setX(this.robot.getGantry().getX() - GANTRY_X_DELTA);
// }
// Robot Lift
if (raiseRobotLift) {
this.robot.getLift().raise();
} else if (liftRobot) {
this.robot.getLift().lift();
if (robotLiftRotation || this.liftArmShouldBeUp) {
this.liftArmShouldBeUp = true;
if (this.robot.getGantry().isIn()) {
this.robot.getGantry().armOut();
} else {
if (robotLiftExtend) {
this.robot.getLift().extend();
}
this.robot.getLift().up();
}
}
*/
if (!robotLiftExtend && previousRobotLiftExtend) {
this.robot.getLift().retract();
}
if (robotLiftReset) {
this.robot.getLift().startReset();
} else if (previousRobotLiftReset) {
this.liftArmShouldBeUp = false;
this.robot.getLift().stopReset();
}
this.robot.update();
this.previousSlideUp = slideUp;
this.previousScrewArmToggle = screwArmToggle;
this.previousScrewDeposit = screwDeposit;
this.previousScrewReset = screwReset;
this.previousRobotLiftReset = robotLiftReset;
this.previousRobotLiftExtend = robotLiftExtend;
}
}

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java
View File

@ -8,9 +8,12 @@ import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_MIN;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_NAME;
import com.arcrobotics.ftclib.controller.PController;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.robotcore.external.Telemetry;
public class Claw {
private final Servo claw;
private final Servo armLeft;
@ -33,14 +36,6 @@ public class Claw {
this.claw.setPosition(CLAW_MIN);
}
public void up() {
this.setArmPosition(PICKUP_ARM_MAX);
}
public void down() {
this.setArmPosition(PICKUP_ARM_MIN);
}
public void setArmPosition(double target) {
target = Math.min(PICKUP_ARM_MAX, Math.max(PICKUP_ARM_MIN, target));
this.armLeft.setPosition(target);

64
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java
View File

@ -1,28 +1,42 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_DELTA_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_KP;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_MIN;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_CENTER;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_SCREW_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_SCREW_POSITIONS;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_X_NAME;
import com.arcrobotics.ftclib.controller.PController;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.robotcore.external.Telemetry;
public class Gantry extends Slide {
private final Servo xServo;
private final Servo armServo;
private final Servo screwServo;
private int currentScrewIndex = 0;
private final CRServo screwServo;
PController armController = new PController(GANTRY_ARM_KP);
private double armControllerTarget;
private Telemetry telemetry;
public Gantry(HardwareMap hardwareMap) {
super(hardwareMap);
this.xServo = hardwareMap.get(Servo.class, GANTRY_X_NAME);
this.armServo = hardwareMap.get(Servo.class, GANTRY_ARM_NAME);
this.screwServo = hardwareMap.get(Servo.class, GANTRY_SCREW_NAME);
this.screwServo.setPosition(1);
this.screwServo = hardwareMap.get(CRServo.class, GANTRY_SCREW_NAME);
this.armServo.setPosition(GANTRY_ARM_MIN);
}
public Gantry(HardwareMap hardwareMap, Telemetry telemetry) {
this(hardwareMap);
this.telemetry = telemetry;
}
public void setX(double x) {
@ -34,23 +48,49 @@ public class Gantry extends Slide {
}
public void armOut() {
this.armServo.setPosition(GANTRY_ARM_MAX);
this.armControllerTarget = GANTRY_ARM_MAX;
}
public void armIn() {
this.armServo.setPosition(GANTRY_ARM_MIN);
this.armControllerTarget = GANTRY_ARM_MIN;
}
public void resetScrew() {
this.screwServo.setPosition(0);
public void intake() {
this.screwServo.setPower(1);
this.screwServo.setDirection(DcMotorSimple.Direction.FORWARD);
}
public void setScrew(double target) {
double clamped = Math.min(1, Math.max(0, target));
this.screwServo.setPosition(clamped);
public void deposit() {
this.screwServo.setPower(1);
this.screwServo.setDirection(DcMotorSimple.Direction.REVERSE);
}
public void stop() {
this.screwServo.setPower(0);
}
public void center() {
this.setX(GANTRY_CENTER);
}
public boolean isIn() {
double fudge = (GANTRY_ARM_MAX - GANTRY_ARM_MIN) * .75;
return this.armServo.getPosition() < GANTRY_ARM_MIN + fudge;
}
public void update() {
this.armController.setSetPoint(this.armControllerTarget);
this.armController.setTolerance(0.001);
this.armController.setP(GANTRY_ARM_KP);
double output = 0;
if (!this.armController.atSetPoint()) {
output = Math.max(-1 * GANTRY_ARM_DELTA_MAX, Math.min(GANTRY_ARM_DELTA_MAX, this.armController.calculate(armServo.getPosition())));
this.armServo.setPosition(this.armServo.getPosition() + output);
}
this.telemetry.addData("Arm P Controller", output);
this.telemetry.addData("Arm P Setpoint", this.armControllerTarget);
this.telemetry.addData("Arm In", this.isIn());
}
}

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
View File

@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.hardware;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
import lombok.Getter;
@ -23,15 +24,24 @@ public class Robot {
@Getter
private RobotLift lift;
public Robot(HardwareMap hardwareMap) {
private Telemetry telemetry;
public Robot(HardwareMap hardwareMap, Telemetry telemetry) {
this.telemetry = telemetry;
this.init(hardwareMap);
}
private void init(HardwareMap hardwareMap) {
// this.drive = new MecanumDrive(hardwareMap);
this.drive = new Drive(hardwareMap);
// this.gantry = new Gantry(hardwareMap);
this.gantry = new Gantry(hardwareMap, telemetry);
this.claw = new Claw(hardwareMap);
// this.lift = new RobotLift(hardwareMap);
this.lift = new RobotLift(hardwareMap, telemetry);
}
public void update() {
this.gantry.update();
this.lift.update();
this.telemetry.update();
}
}

25
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java
View File

@ -24,22 +24,29 @@ public class RobotConstants {
public static int SLIDE_UP = 100;
// Arm
public static double PICKUP_ARM_MIN = 0.19;
public static double PICKUP_ARM_MAX = 0.75;
public static double PICKUP_ARM_MIN = 0.17;
public static double PICKUP_ARM_MAX = 0.76;
public static double CLAW_MIN = 0.9;
public static double CLAW_MAX = 0.6;
public static double CLAW_ARM_DELTA = 0.005;
public static double CLAW_ARM_DELTA = 0.008;
// Gantry
public static double GANTRY_ARM_MIN = 0;
public static double GANTRY_ARM_MAX = 0;
public static double[] GANTRY_SCREW_POSITIONS = new double[] { 0, 0.1, 0.2, 1.0 };
public static double GANTRY_ARM_MIN = 0.43;
public static double GANTRY_ARM_MAX = 0.95;
public static int GANTRY_LIFT_DELTA = 50;
public static double GANTRY_X_DELTA = 0.01;
public static double GANTRY_CENTER = 0.5;
public static double GANTRY_SCREW_DELTA = 0.01;
public static double GANTRY_ARM_KP = 0.03;
public static double GANTRY_ARM_KI = 0f;
public static double GANTRY_ARM_KD = 0f;
public static double GANTRY_ARM_DELTA_MAX = 0.0025;
// Robot Lift
public static int LIFT_ROTATION_UP = 100;
public static int LIFT_EXTEND_MAX = 100;
public static double LIFT_ROTATION_UP = 0.4;
public static double LIFT_ROTATION_DOWN = 0;
public static int LIFT_EXTEND_MAX = 13100;
public static double LIFT_RETRACT_PCT = 0.1;
public static double LIFT_ARM_KP = 0.038;
public static double LIFT_POWER = 1f;
}

69
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotLift.java
View File

@ -1,51 +1,88 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_ARM_KP;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_EXTEND_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_RETRACT_PCT;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_ROTATION_DOWN;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_ROTATION_UP;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.ROBOT_LIFT_LIFT_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.ROBOT_LIFT_ROTATION_NAME;
import com.arcrobotics.ftclib.controller.PController;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.robotcore.external.Telemetry;
public class RobotLift {
private DcMotor rotation;
private Servo rotation;
private DcMotor lift;
PController armController = new PController(LIFT_ARM_KP);
private double armControllerTarget;
private Telemetry telemetry;
public RobotLift(HardwareMap hardwareMap) {
this.init(hardwareMap);
}
public RobotLift(HardwareMap hardwareMap, Telemetry telemetry) {
this(hardwareMap);
this.telemetry = telemetry;
}
public void init(HardwareMap hardwareMap) {
this.rotation = hardwareMap.get(DcMotor.class, ROBOT_LIFT_ROTATION_NAME);
this.rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.rotation = hardwareMap.get(Servo.class, ROBOT_LIFT_ROTATION_NAME);
this.lift = hardwareMap.get(DcMotor.class, ROBOT_LIFT_LIFT_NAME);
this.lift.setTargetPosition(0);
this.lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.lift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
}
public void raise() {
this.rotation.setTargetPosition(LIFT_ROTATION_UP);
public void up() {
this.armControllerTarget = LIFT_ROTATION_UP;
}
public void extend() {
this.armControllerTarget = LIFT_ROTATION_UP;
this.lift.setTargetPosition(LIFT_EXTEND_MAX);
this.rotation.setPower(1);
this.lift.setPower(1);
}
public void lift() {
this.rotation.setTargetPosition(LIFT_ROTATION_UP);
this.lift.setTargetPosition(0);
public void retract() {
this.armControllerTarget = LIFT_ROTATION_UP;
int liftTarget = (int)(LIFT_EXTEND_MAX * (1 - LIFT_RETRACT_PCT));
int target = this.lift.getCurrentPosition() < liftTarget ? 0 : liftTarget;
this.lift.setTargetPosition(target);
this.lift.setPower(1);
this.rotation.setPower(1);
}
public void reset() {
this.rotation.setTargetPosition(0);
this.lift.setTargetPosition(0);
public void startReset() {
this.armControllerTarget = LIFT_ROTATION_DOWN;
this.lift.setTargetPosition(-1 * LIFT_EXTEND_MAX);
this.lift.setPower(1);
}
this.lift.setPower(0.25);
this.rotation.setPower(0.25);
public void stopReset() {
this.armControllerTarget = LIFT_ROTATION_DOWN;
this.lift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
this.lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.lift.setPower(0);
}
public void update() {
this.armController.setP(LIFT_ARM_KP);
this.armController.setSetPoint(this.armControllerTarget);
double output = this.armController.calculate(this.rotation.getPosition());
this.rotation.setPosition(this.rotation.getPosition() + output);
this.telemetry.addData("Lift P Controller", output);
}
public boolean isUp() {
return this.armControllerTarget == LIFT_ROTATION_UP;
}
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slide.java
View File

@ -21,12 +21,12 @@ public class Slide {
}
public void setTarget(int target) {
this.left.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.left.setTargetPosition(target);
this.left.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.left.setPower(1);
this.right.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.right.setTargetPosition(target);
this.right.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.right.setPower(1);
}

5
build.common.gradle
View File

@ -57,6 +57,7 @@ android {
minSdkVersion 24
//noinspection ExpiredTargetSdkVersion
targetSdkVersion 28
multiDexEnabled true
/**
* We keep the versionCode and versionName of robot controller applications in sync with
@ -93,14 +94,14 @@ android {
signingConfig signingConfigs.release
ndk {
abiFilters "armeabi-v7a", "arm64-v8a"
abiFilters "armeabi-v7a"
}
}
debug {
debuggable true
jniDebuggable true
ndk {
abiFilters "armeabi-v7a", "arm64-v8a"
abiFilters "armeabi-v7a"
}
}
}