From e30ee0f37db1111d7401cf6f1bb3ec6f9226e7fb Mon Sep 17 00:00:00 2001 From: Philip Smith Date: Mon, 16 Nov 2020 16:43:10 -0500 Subject: [PATCH 1/6] made stuff work --- .../teamcode/actions/TeleopDriveAction.java | 17 ++--- .../ftc/teamcode/mechanisms/Intake.java | 6 +- .../ftc/teamcode/mechanisms/Shooter.java | 62 +++++++++++++++---- .../ftc/teamcode/mechanisms/Transfer.java | 4 +- .../mechanisms/tests/QQ_DualTest.java | 25 ++++++++ .../mechanisms/tests/QQ_TestAnalogSensor.java | 2 +- 6 files changed, 90 insertions(+), 26 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/tests/QQ_DualTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java index 1282d9d..22f233c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java @@ -6,7 +6,7 @@ import org.firstinspires.ftc.teamcode.opModes.QQ_Opmode; public class TeleopDriveAction extends QQ_Action { - double pivotAngle; + double pivotAngle = 0; boolean wasUp; boolean wasDown; final static double MAX_SPEED = 1; @@ -29,7 +29,7 @@ public QQ_Action run(QQ_Opmode opmode) { void manipulatorControls(QQ_Opmode opmode) { //spinning shooter wheels if (opmode.qq_gamepad2.rightBumper()) { - opmode.robot.shooter.spinWheels(0.8, 0.5); + opmode.robot.shooter.spinWheels(-1, -0.8); } else { opmode.robot.shooter.spinWheels(0, 0); } @@ -41,18 +41,19 @@ void manipulatorControls(QQ_Opmode opmode) { } else { opmode.robot.shooter.flick(false); } - +/* //up and down of pivot shooter if (opmode.qq_gamepad2.dpadUp() && !wasUp) { - pivotAngle = Math.max(1.0, pivotAngle + 0.1); + pivotAngle = Math.min(15, pivotAngle + 3); } wasUp = opmode.qq_gamepad2.dpadUp(); if (opmode.qq_gamepad2.dpadDown() && !wasDown) { - pivotAngle = Math.min(0, pivotAngle - 0.1); + pivotAngle = Math.max(0, pivotAngle - 3); } wasDown = opmode.qq_gamepad2.dpadDown(); - opmode.robot.shooter.setPivotAngle(pivotAngle); + opmode.telemetry.addData("angle", pivotAngle); + opmode.robot.shooter.goToAngle(pivotAngle); //up and down of wobbly goal if (opmode.qq_gamepad2.leftStick.getY() > 0.2) { @@ -67,12 +68,12 @@ void manipulatorControls(QQ_Opmode opmode) { } else { opmode.robot.wobblyGoal.closeGrabber(); } - +*/ //intake and transfer in and out if (opmode.qq_gamepad2.rightStick.getY() > 0.2) { opmode.robot.intake.changeState(Intake.intakeState.Start); opmode.robot.transfer.changeTransfer(Transfer.transferState.Start); - } else if (opmode.qq_gamepad2.rightStick.getY() < 0.2) { + } else if (opmode.qq_gamepad2.rightStick.getY() < -0.2) { opmode.robot.intake.changeState(Intake.intakeState.Reverse); opmode.robot.transfer.changeTransfer(Transfer.transferState.Reverse); } else { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Intake.java index 352b337..5956cee 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Intake.java @@ -17,8 +17,8 @@ public enum intakeState{ }; private DcMotor intakeMotor; - private static final double intakeSpeed = 0.2; - private static final double reverseIntakeSpeed = -0.2; + private static final double intakeSpeed = -0.5; + private static final double reverseIntakeSpeed = 0.2; /** * initialize intake @@ -35,7 +35,7 @@ public void init(HardwareMap hwMap) { */ @Override public List getTests() { - return Arrays.asList(new QQ_TestMotor("Intake Motor", 0.25, intakeMotor)); + return Arrays.asList(new QQ_TestMotor("Intake Motor", -0.5, intakeMotor)); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Shooter.java index 9d9cd04..42367a2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Shooter.java @@ -1,11 +1,14 @@ package org.firstinspires.ftc.teamcode.mechanisms; +import com.qualcomm.robotcore.hardware.AnalogInput; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.Range; +import org.firstinspires.ftc.teamcode.mechanisms.tests.QQ_DualTest; import org.firstinspires.ftc.teamcode.mechanisms.tests.QQ_Test; +import org.firstinspires.ftc.teamcode.mechanisms.tests.QQ_TestAnalogSensor; import org.firstinspires.ftc.teamcode.mechanisms.tests.QQ_TestMotor; import org.firstinspires.ftc.teamcode.mechanisms.tests.QQ_TestServo; @@ -22,16 +25,33 @@ enum AimLocation { PowerShot3 } + public double shooterAngle = 0; + private DcMotor shooterBack; private DcMotor shooterFront; private Servo shooterPivot; private Servo shooterImport; + private AnalogInput pot; final double PIVOT_DOWN = 0.25; // TODO: find right value final double PIVOT_UP = 0.75; // TODO: Find correct value - final double INSERT = 0.25; // TODO: Find Value - final double RESET = 0.75; // TODO: Find value + + final double ANGLE_UP = 15.0; + final double ANGLE_DOWN = 0.0; + + final double VOLT_UP = 0.17; + final double VOLT_DOWN = 0.27; + + final double VOLTAGE_TOLERANCE = 0.03; + final double VOLTAGE_KP_UP = 1.6; + final double VOLTAGE_KP_DOWN = 0.8; + + + + + final double INSERT = 0; // TODO: Find Value + final double RESET = 0.18; // TODO: Find value /** * initializes Shooter @@ -44,6 +64,8 @@ public void init(HardwareMap hwMap) { shooterFront = hwMap.get(DcMotor.class, "shooter_front_motor"); shooterPivot = hwMap.get(Servo.class, "servo_pivot_shooter"); shooterImport = hwMap.get(Servo.class, "servo_import_shooter"); + pot = hwMap.get(AnalogInput.class, "lift_pot"); + } /** @@ -55,7 +77,9 @@ public void init(HardwareMap hwMap) { public List getTests() { return Arrays.asList(new QQ_TestMotor("Front Motor", 0.25, shooterFront), new QQ_TestMotor("Back Motor", 0.25, shooterBack), - new QQ_TestServo("Pivot", PIVOT_UP, PIVOT_DOWN, shooterPivot), + new QQ_DualTest( + new QQ_TestServo("Pivot", PIVOT_UP, PIVOT_DOWN, shooterPivot), + new QQ_TestAnalogSensor("pot", pot)), new QQ_TestServo("Import", INSERT, RESET, shooterImport)); } @@ -71,6 +95,29 @@ public boolean aim(AimLocation aimLocation) { return false; } + + public boolean goToAngle(double angle){ + return goToVoltage(Range.scale(angle, VOLT_DOWN, VOLT_UP, ANGLE_DOWN, ANGLE_UP)); + } + + private boolean goToVoltage(double desiredVoltage){ + double voltageDiff = pot.getVoltage() - desiredVoltage; + if (Math.abs(voltageDiff) <= VOLTAGE_TOLERANCE){ + shooterPivot.setPosition(0.0); + return true; + } + + if(voltageDiff > 0){ + double speed = Math.max( voltageDiff * VOLTAGE_KP_UP, 0.75); + shooterPivot.setPosition(speed); + } else { + double speed = Math.max( voltageDiff * VOLTAGE_KP_DOWN, 0.25); + shooterPivot.setPosition(speed); + } + return false; + } + + public void spinWheels(double frontSpeed, double backSpeed) { shooterBack.setPower(frontSpeed); shooterFront.setPower(backSpeed); @@ -84,15 +131,6 @@ public void flick(boolean shouldFlick){ } } - /** - * sets shooter pivot - * @param angle 0 is down, 1 is up - */ - public void setPivotAngle(double angle){ - - shooterPivot.setPosition(Range.scale(angle, 0, 1, PIVOT_DOWN, PIVOT_UP)); - - } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Transfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Transfer.java index 036601e..35f4d29 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Transfer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Transfer.java @@ -23,7 +23,7 @@ public enum transferState { */ private DcMotor transferMotor; - private static final double transferSpeed = 0.25; + private static final double transferSpeed = 0.5; private static final double reverseTransferSpeed = -0.25; @Override @@ -38,7 +38,7 @@ public void init(HardwareMap hwMap) { */ @Override public List getTests() { - return Arrays.asList(new QQ_TestMotor("Transfer Motor", 0.25, transferMotor)); + return Arrays.asList(new QQ_TestMotor("Transfer Motor", 0.5, transferMotor)); } public void changeTransfer(Transfer.transferState desiredState) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/tests/QQ_DualTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/tests/QQ_DualTest.java new file mode 100644 index 0000000..c9d8a11 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/tests/QQ_DualTest.java @@ -0,0 +1,25 @@ +package org.firstinspires.ftc.teamcode.mechanisms.tests; + +import com.qualcomm.robotcore.hardware.AnalogInput; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +public class QQ_DualTest extends QQ_Test { + QQ_Test qq_test_1; + QQ_Test qq_test_2; + + + public QQ_DualTest(QQ_Test qq_test_1, QQ_Test qq_test_2){ + super(qq_test_1.getDescription() + qq_test_2.getDescription()); + this.qq_test_1 = qq_test_1; + this.qq_test_2 = qq_test_2; + + } + + @Override + public void run(boolean on, Telemetry telemetry) { + qq_test_1.run(on, telemetry); + qq_test_2.run(on, telemetry); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/tests/QQ_TestAnalogSensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/tests/QQ_TestAnalogSensor.java index 9de59ea..b6c96cf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/tests/QQ_TestAnalogSensor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/tests/QQ_TestAnalogSensor.java @@ -13,7 +13,7 @@ public class QQ_TestAnalogSensor extends QQ_Test { * @param description a human readable description of the test * @param analogInput an instance of a analog input sensor the test is using */ - QQ_TestAnalogSensor(String description, AnalogInput analogInput) { + public QQ_TestAnalogSensor(String description, AnalogInput analogInput) { super(description); this.analogInput = analogInput; } From c957b5729585d4318a8ef8ca8468312788a834e0 Mon Sep 17 00:00:00 2001 From: Philip Smith Date: Mon, 16 Nov 2020 16:43:10 -0500 Subject: [PATCH 2/6] made stuff work --- .../teamcode/actions/TeleopDriveAction.java | 17 ++--- .../ftc/teamcode/mechanisms/Intake.java | 6 +- .../ftc/teamcode/mechanisms/Shooter.java | 62 +++++++++++++++---- .../ftc/teamcode/mechanisms/Transfer.java | 4 +- .../mechanisms/tests/QQ_DualTest.java | 25 ++++++++ .../mechanisms/tests/QQ_TestAnalogSensor.java | 2 +- 6 files changed, 90 insertions(+), 26 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/tests/QQ_DualTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java index e092ce6..c718f10 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java @@ -9,7 +9,7 @@ import org.firstinspires.ftc.teamcode.utils.RobotPose; public class TeleopDriveAction extends QQ_Action { - double pivotAngle; + double pivotAngle = 0; boolean wasUp; boolean wasDown; final static double MAX_SPEED = 1; @@ -35,7 +35,7 @@ public QQ_Action run(QQ_Opmode opmode) { void manipulatorControls(QQ_Opmode opmode) { //spinning shooter wheels if (opmode.qq_gamepad2.rightBumper()) { - opmode.robot.shooter.spinWheels(0.8, 0.5); + opmode.robot.shooter.spinWheels(-1, -0.8); } else { opmode.robot.shooter.spinWheels(0, 0); } @@ -47,18 +47,19 @@ void manipulatorControls(QQ_Opmode opmode) { } else { opmode.robot.shooter.flick(false); } - +/* //up and down of pivot shooter if (opmode.qq_gamepad2.dpadUp() && !wasUp) { - pivotAngle = Math.max(1.0, pivotAngle + 0.1); + pivotAngle = Math.min(15, pivotAngle + 3); } wasUp = opmode.qq_gamepad2.dpadUp(); if (opmode.qq_gamepad2.dpadDown() && !wasDown) { - pivotAngle = Math.min(0, pivotAngle - 0.1); + pivotAngle = Math.max(0, pivotAngle - 3); } wasDown = opmode.qq_gamepad2.dpadDown(); - opmode.robot.shooter.setPivotAngle(pivotAngle); + opmode.telemetry.addData("angle", pivotAngle); + opmode.robot.shooter.goToAngle(pivotAngle); //up and down of wobbly goal if (opmode.qq_gamepad2.leftStick.getY() > 0.2) { @@ -73,12 +74,12 @@ void manipulatorControls(QQ_Opmode opmode) { } else { opmode.robot.wobblyGoal.closeGrabber(); } - +*/ //intake and transfer in and out if (opmode.qq_gamepad2.rightStick.getY() > 0.2) { opmode.robot.intake.changeState(Intake.intakeState.Start); opmode.robot.transfer.changeTransfer(Transfer.transferState.Start); - } else if (opmode.qq_gamepad2.rightStick.getY() < 0.2) { + } else if (opmode.qq_gamepad2.rightStick.getY() < -0.2) { opmode.robot.intake.changeState(Intake.intakeState.Reverse); opmode.robot.transfer.changeTransfer(Transfer.transferState.Reverse); } else { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Intake.java index 352b337..5956cee 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Intake.java @@ -17,8 +17,8 @@ public enum intakeState{ }; private DcMotor intakeMotor; - private static final double intakeSpeed = 0.2; - private static final double reverseIntakeSpeed = -0.2; + private static final double intakeSpeed = -0.5; + private static final double reverseIntakeSpeed = 0.2; /** * initialize intake @@ -35,7 +35,7 @@ public void init(HardwareMap hwMap) { */ @Override public List getTests() { - return Arrays.asList(new QQ_TestMotor("Intake Motor", 0.25, intakeMotor)); + return Arrays.asList(new QQ_TestMotor("Intake Motor", -0.5, intakeMotor)); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Shooter.java index 9d9cd04..42367a2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Shooter.java @@ -1,11 +1,14 @@ package org.firstinspires.ftc.teamcode.mechanisms; +import com.qualcomm.robotcore.hardware.AnalogInput; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.Range; +import org.firstinspires.ftc.teamcode.mechanisms.tests.QQ_DualTest; import org.firstinspires.ftc.teamcode.mechanisms.tests.QQ_Test; +import org.firstinspires.ftc.teamcode.mechanisms.tests.QQ_TestAnalogSensor; import org.firstinspires.ftc.teamcode.mechanisms.tests.QQ_TestMotor; import org.firstinspires.ftc.teamcode.mechanisms.tests.QQ_TestServo; @@ -22,16 +25,33 @@ enum AimLocation { PowerShot3 } + public double shooterAngle = 0; + private DcMotor shooterBack; private DcMotor shooterFront; private Servo shooterPivot; private Servo shooterImport; + private AnalogInput pot; final double PIVOT_DOWN = 0.25; // TODO: find right value final double PIVOT_UP = 0.75; // TODO: Find correct value - final double INSERT = 0.25; // TODO: Find Value - final double RESET = 0.75; // TODO: Find value + + final double ANGLE_UP = 15.0; + final double ANGLE_DOWN = 0.0; + + final double VOLT_UP = 0.17; + final double VOLT_DOWN = 0.27; + + final double VOLTAGE_TOLERANCE = 0.03; + final double VOLTAGE_KP_UP = 1.6; + final double VOLTAGE_KP_DOWN = 0.8; + + + + + final double INSERT = 0; // TODO: Find Value + final double RESET = 0.18; // TODO: Find value /** * initializes Shooter @@ -44,6 +64,8 @@ public void init(HardwareMap hwMap) { shooterFront = hwMap.get(DcMotor.class, "shooter_front_motor"); shooterPivot = hwMap.get(Servo.class, "servo_pivot_shooter"); shooterImport = hwMap.get(Servo.class, "servo_import_shooter"); + pot = hwMap.get(AnalogInput.class, "lift_pot"); + } /** @@ -55,7 +77,9 @@ public void init(HardwareMap hwMap) { public List getTests() { return Arrays.asList(new QQ_TestMotor("Front Motor", 0.25, shooterFront), new QQ_TestMotor("Back Motor", 0.25, shooterBack), - new QQ_TestServo("Pivot", PIVOT_UP, PIVOT_DOWN, shooterPivot), + new QQ_DualTest( + new QQ_TestServo("Pivot", PIVOT_UP, PIVOT_DOWN, shooterPivot), + new QQ_TestAnalogSensor("pot", pot)), new QQ_TestServo("Import", INSERT, RESET, shooterImport)); } @@ -71,6 +95,29 @@ public boolean aim(AimLocation aimLocation) { return false; } + + public boolean goToAngle(double angle){ + return goToVoltage(Range.scale(angle, VOLT_DOWN, VOLT_UP, ANGLE_DOWN, ANGLE_UP)); + } + + private boolean goToVoltage(double desiredVoltage){ + double voltageDiff = pot.getVoltage() - desiredVoltage; + if (Math.abs(voltageDiff) <= VOLTAGE_TOLERANCE){ + shooterPivot.setPosition(0.0); + return true; + } + + if(voltageDiff > 0){ + double speed = Math.max( voltageDiff * VOLTAGE_KP_UP, 0.75); + shooterPivot.setPosition(speed); + } else { + double speed = Math.max( voltageDiff * VOLTAGE_KP_DOWN, 0.25); + shooterPivot.setPosition(speed); + } + return false; + } + + public void spinWheels(double frontSpeed, double backSpeed) { shooterBack.setPower(frontSpeed); shooterFront.setPower(backSpeed); @@ -84,15 +131,6 @@ public void flick(boolean shouldFlick){ } } - /** - * sets shooter pivot - * @param angle 0 is down, 1 is up - */ - public void setPivotAngle(double angle){ - - shooterPivot.setPosition(Range.scale(angle, 0, 1, PIVOT_DOWN, PIVOT_UP)); - - } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Transfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Transfer.java index 036601e..35f4d29 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Transfer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Transfer.java @@ -23,7 +23,7 @@ public enum transferState { */ private DcMotor transferMotor; - private static final double transferSpeed = 0.25; + private static final double transferSpeed = 0.5; private static final double reverseTransferSpeed = -0.25; @Override @@ -38,7 +38,7 @@ public void init(HardwareMap hwMap) { */ @Override public List getTests() { - return Arrays.asList(new QQ_TestMotor("Transfer Motor", 0.25, transferMotor)); + return Arrays.asList(new QQ_TestMotor("Transfer Motor", 0.5, transferMotor)); } public void changeTransfer(Transfer.transferState desiredState) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/tests/QQ_DualTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/tests/QQ_DualTest.java new file mode 100644 index 0000000..c9d8a11 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/tests/QQ_DualTest.java @@ -0,0 +1,25 @@ +package org.firstinspires.ftc.teamcode.mechanisms.tests; + +import com.qualcomm.robotcore.hardware.AnalogInput; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +public class QQ_DualTest extends QQ_Test { + QQ_Test qq_test_1; + QQ_Test qq_test_2; + + + public QQ_DualTest(QQ_Test qq_test_1, QQ_Test qq_test_2){ + super(qq_test_1.getDescription() + qq_test_2.getDescription()); + this.qq_test_1 = qq_test_1; + this.qq_test_2 = qq_test_2; + + } + + @Override + public void run(boolean on, Telemetry telemetry) { + qq_test_1.run(on, telemetry); + qq_test_2.run(on, telemetry); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/tests/QQ_TestAnalogSensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/tests/QQ_TestAnalogSensor.java index 9de59ea..b6c96cf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/tests/QQ_TestAnalogSensor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/tests/QQ_TestAnalogSensor.java @@ -13,7 +13,7 @@ public class QQ_TestAnalogSensor extends QQ_Test { * @param description a human readable description of the test * @param analogInput an instance of a analog input sensor the test is using */ - QQ_TestAnalogSensor(String description, AnalogInput analogInput) { + public QQ_TestAnalogSensor(String description, AnalogInput analogInput) { super(description); this.analogInput = analogInput; } From 1ad62532988f397b66004185304f2c4fc7547dcd Mon Sep 17 00:00:00 2001 From: Philip Smith Date: Mon, 16 Nov 2020 19:26:15 -0500 Subject: [PATCH 3/6] tried to fix rotator --- .../ftc/teamcode/actions/TeleopDriveAction.java | 9 +++++++++ .../ftc/teamcode/mechanisms/Navigation.java | 6 ++++++ 2 files changed, 15 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java index c718f10..ecd9631 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java @@ -96,6 +96,15 @@ void manipulatorControls(QQ_Opmode opmode) { public void driverControls(QQ_Opmode opmode) { opmode.qq_gamepad1.leftStick.setSquared(true); + + + + if (opmode.qq_gamepad1.dpadUp()){ + opmode.robot.nav.setImuOffset(0, AngleUnit.RADIANS); + } + + + if (opmode.qq_gamepad1.leftBumper()) { opmode.robot.mecanumDrive.setMaxSpeed(SLOW_SPEED); } else if (opmode.qq_gamepad1.rightBumper()) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Navigation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Navigation.java index dab0af0..4a4071a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Navigation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Navigation.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.mechanisms; +import android.view.accessibility.AccessibilityNodeInfo; + import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.util.Range; @@ -68,6 +70,10 @@ public void setCurrentPosition(RobotPose pose){ currentPosition = pose; } + public void setImuOffset(double imuOffset, AngleUnit au) { + this.imuOffset = au.toRadians(imuOffset); + } + private double getHeading(AngleUnit au){ Orientation angles; From f4c8688b7ca22171b70f526d42a7107683aed07e Mon Sep 17 00:00:00 2001 From: Philip Smith Date: Mon, 16 Nov 2020 21:41:46 -0500 Subject: [PATCH 4/6] idk what happened this stuff doesn't work but im commiting so its not on my local machine --- .../org/firstinspires/ftc/teamcode/actions/ShootAction.java | 2 +- .../firstinspires/ftc/teamcode/actions/TeleopDriveAction.java | 2 +- .../org/firstinspires/ftc/teamcode/mechanisms/Navigation.java | 2 +- .../org/firstinspires/ftc/teamcode/opModes/QQ_ParkAuto.java | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/ShootAction.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/ShootAction.java index 8f0e078..2242e19 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/ShootAction.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/ShootAction.java @@ -15,7 +15,7 @@ public QQ_Action run(QQ_Opmode opmode) { switch (step){ case 0: opmode.robot.shooter.spinWheels(0.8, 0.5); - opmode.robot.shooter.setPivotAngle(AUTO_SHOOT_ANGLE); + //opmode.robot.shooter.setPivotAngle(AUTO_SHOOT_ANGLE); waitTime = opmode.time + 0.5; step = 1; break; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java index ecd9631..2d28b60 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java @@ -100,7 +100,7 @@ public void driverControls(QQ_Opmode opmode) { if (opmode.qq_gamepad1.dpadUp()){ - opmode.robot.nav.setImuOffset(0, AngleUnit.RADIANS); + opmode.robot.nav.setImuOffset(-(opmode.robot.nav.getHeading(AngleUnit.RADIANS) + (Math.PI) ), AngleUnit.RADIANS); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Navigation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Navigation.java index 4a4071a..5102602 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Navigation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Navigation.java @@ -74,7 +74,7 @@ public void setImuOffset(double imuOffset, AngleUnit au) { this.imuOffset = au.toRadians(imuOffset); } - private double getHeading(AngleUnit au){ + public double getHeading(AngleUnit au){ Orientation angles; angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.RADIANS); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/QQ_ParkAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/QQ_ParkAuto.java index e31a2c4..1300cf6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/QQ_ParkAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/QQ_ParkAuto.java @@ -14,7 +14,7 @@ public class QQ_ParkAuto extends QQ_Opmode { double START_X = 48; double TOLERANCE = 0.5; double START_Y = 9; - double PARK_Y = 80; + double PARK_Y = 80.0 / 2; @Override public void init() { super.init(); From 540b6a0699e2e4ff99fcd3f7c950b1f2a3a4c563 Mon Sep 17 00:00:00 2001 From: Philip Smith Date: Thu, 19 Nov 2020 13:08:56 -0500 Subject: [PATCH 5/6] Fixed field relative driving --- .../teamcode/actions/TeleopDriveAction.java | 2 +- .../ftc/teamcode/mechanisms/Navigation.java | 22 +++++++++---------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java index 2d28b60..a92d9f4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java @@ -100,7 +100,7 @@ public void driverControls(QQ_Opmode opmode) { if (opmode.qq_gamepad1.dpadUp()){ - opmode.robot.nav.setImuOffset(-(opmode.robot.nav.getHeading(AngleUnit.RADIANS) + (Math.PI) ), AngleUnit.RADIANS); + opmode.robot.nav.resetIMU(0, AngleUnit.DEGREES); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Navigation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Navigation.java index 5102602..75d8178 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Navigation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Navigation.java @@ -1,7 +1,5 @@ package org.firstinspires.ftc.teamcode.mechanisms; -import android.view.accessibility.AccessibilityNodeInfo; - import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.util.Range; @@ -19,18 +17,17 @@ public class Navigation { // Public Classe - public RobotPose currentPosition; - public BNO055IMU imu; - private double imuOffset; + public static RobotPose currentPosition; + // Private Classes + private static BNO055IMU imu; MecanumDrive mecanumDrive; - double TRANSLATE_KP = 0.1; - double MIN_R = 0.14; - - // Private Values private double angleTolerance = AngleUnit.RADIANS.fromDegrees(2); + private static double imuOffset; + double TRANSLATE_KP = 0.1; + double MIN_R = 0.14; /** * Constructor @@ -70,8 +67,11 @@ public void setCurrentPosition(RobotPose pose){ currentPosition = pose; } - public void setImuOffset(double imuOffset, AngleUnit au) { - this.imuOffset = au.toRadians(imuOffset); + public void resetIMU(double angle, AngleUnit au) { + double supposedHeading = au.toRadians(angle); + double currentHeading = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.RADIANS).firstAngle; //if we use our get method it would double the offset if we set it again + + imuOffset = supposedHeading - currentHeading; } public double getHeading(AngleUnit au){ From baf0d7f3cafd3c908fcd12be35694c8407b7542a Mon Sep 17 00:00:00 2001 From: Philip Smith Date: Thu, 19 Nov 2020 13:13:20 -0500 Subject: [PATCH 6/6] added other directions on the dpad --- .../ftc/teamcode/actions/TeleopDriveAction.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java index a92d9f4..7bda70c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java @@ -101,6 +101,12 @@ public void driverControls(QQ_Opmode opmode) { if (opmode.qq_gamepad1.dpadUp()){ opmode.robot.nav.resetIMU(0, AngleUnit.DEGREES); + } else if (opmode.qq_gamepad1.dpadLeft()){ + opmode.robot.nav.resetIMU(-90, AngleUnit.DEGREES); + } else if (opmode.qq_gamepad1.dpadDown()){ + opmode.robot.nav.resetIMU(180, AngleUnit.DEGREES); + } else if (opmode.qq_gamepad1.dpadRight()){ + opmode.robot.nav.resetIMU(90, AngleUnit.DEGREES); }