From 9201c65d42967a501fb0baa7373a8dad5e08c654 Mon Sep 17 00:00:00 2001 From: Keerthana Date: Sat, 12 Oct 2024 16:11:45 -0400 Subject: [PATCH 1/4] added arm code and made teleop for scrimmage --- .../ftc/teamcode/ftc16072/Mechanisms/Arm.java | 71 +++++++++++++++++++ .../ftc16072/OpModes/ScrimmageTeleop.java | 38 ++++++++++ .../ftc/teamcode/ftc16072/Robot.java | 6 +- 3 files changed, 114 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Arm.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ScrimmageTeleop.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Arm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Arm.java new file mode 100644 index 0000000..46b9033 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Arm.java @@ -0,0 +1,71 @@ +package org.firstinspires.ftc.teamcode.ftc16072.Mechanisms; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.ftc16072.Tests.QQTest; +import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestMotor; +import org.firstinspires.ftc.teamcode.ftc16072.Util.PIDFController; + +import java.util.Arrays; +import java.util.List; + +public class Arm extends QQMechanism{ + public static final double TEST_SPEED = 0.55; + public static double TEST_SPEED_OFF = 0.4; + DcMotor armMotor; + public static double kP = 0.01; + public static double kI = 0.0; + public static double kD = 0.0; + public static double kF = 0.4; + public static double max = 0.8; + public static double min = 0.0; + + public int currentPos; + public int desiredPos; + public double motorPower; + public int PLACEMENT_POSITION = 1000; //TODO make real + public int INTAKE_POSITION = 0; //TODO + + + PIDFController pidfController = new PIDFController(kP,kI,kD,kF,max,min); + + @Override + public void init(HardwareMap hwMap) { + armMotor = hwMap.get(DcMotor.class, "arm_motor"); + armMotor.setDirection(DcMotorSimple.Direction.REVERSE); + armMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + armMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } + + public void setPosition(int desiredPos){ + this.desiredPos = desiredPos; + } + public void manualPositionChange(int changeAmount){ + desiredPos += changeAmount; + } + + public void goToIntake(){ + desiredPos = INTAKE_POSITION; + } + public void goToPlacement(){ + desiredPos = PLACEMENT_POSITION; + } + + @Override + public void update(){ + currentPos = (armMotor.getCurrentPosition()); + double motorPower = pidfController.calculate(desiredPos,currentPos); + this.motorPower = motorPower; + armMotor.setPower(motorPower); + } + + @Override + public List getTests() { + return Arrays.asList( + new TestMotor("arm move", TEST_SPEED, armMotor) + ); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ScrimmageTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ScrimmageTeleop.java new file mode 100644 index 0000000..4b22c40 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ScrimmageTeleop.java @@ -0,0 +1,38 @@ +package org.firstinspires.ftc.teamcode.ftc16072.OpModes; + +import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.MecanumDrive; + +public class ScrimmageTeleop extends QQOpMode{ + + public static final double TRIGGER_THRESHOLD = 0.5; + public static final int MANUAL_CHANGE = 5; + + public void loop(){ + super.loop(); + double forward = -gamepad1.left_stick_y; + double left = gamepad1.left_stick_x; + double rotate = gamepad1.right_stick_x; + + if (gamepad1.left_trigger > TRIGGER_THRESHOLD ){ + robot.mecanumDrive.setSpeed(MecanumDrive.Speed.TURBO); + } else if (gamepad1.left_bumper) { + robot.mecanumDrive.setSpeed(MecanumDrive.Speed.SLOW); + } else robot.mecanumDrive.setSpeed(MecanumDrive.Speed.NORMAL); + + if (gamepad1.a){ + robot.arm.goToIntake(); + }else if (gamepad1.b) { + robot.arm.goToPlacement();} + else if (gamepad1.dpad_up){ + robot.arm.manualPositionChange(MANUAL_CHANGE); + }else if (gamepad1.dpad_down){ + robot.arm.manualPositionChange(-MANUAL_CHANGE); + } + + + if (gamepad1.right_bumper) { + robot.claw.close(); + }else if (gamepad1.right_trigger > TRIGGER_THRESHOLD){ + robot.claw.open();} + } +} 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 7e15556..80118a9 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 @@ -2,6 +2,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Arm; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Claw; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.ControlHub; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.DoubleReverse4Bar; @@ -21,6 +22,7 @@ public class Robot { public Claw claw; public DoubleReverse4Bar doubleReverse4Bar; public Slides slides; + public Arm arm; List mechanisms; public Robot() { @@ -30,6 +32,7 @@ public Robot() { claw = new Claw(); doubleReverse4Bar = new DoubleReverse4Bar(); slides = new Slides(); + arm = new Arm(); mechanisms = Arrays.asList( controlHub, @@ -38,7 +41,8 @@ public Robot() { claw, slides, claw, - doubleReverse4Bar); + doubleReverse4Bar, + arm); } public void init(HardwareMap hwMap) { for (QQMechanism mechanism : mechanisms) { From c8568689ab2ade8bd97e05d7e3c6c774bead45db Mon Sep 17 00:00:00 2001 From: mahin Date: Mon, 14 Oct 2024 20:20:34 -0400 Subject: [PATCH 2/4] Robot configuration for 6 weeks --- TeamCode/src/main/res/xml/ri6w.xml | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) create mode 100644 TeamCode/src/main/res/xml/ri6w.xml diff --git a/TeamCode/src/main/res/xml/ri6w.xml b/TeamCode/src/main/res/xml/ri6w.xml new file mode 100644 index 0000000..0364bf2 --- /dev/null +++ b/TeamCode/src/main/res/xml/ri6w.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + From 2cef6f049afc9f193688de670fe8f1a0b573f79b Mon Sep 17 00:00:00 2001 From: mahin Date: Mon, 14 Oct 2024 20:33:21 -0400 Subject: [PATCH 3/4] First pass of Arm and Claw for Ri6w --- .../ftc/teamcode/ftc16072/Mechanisms/Arm.java | 30 +++++++++++-------- .../teamcode/ftc16072/Mechanisms/Claw.java | 10 +++---- .../teamcode/ftc16072/OpModes/RI3WTeleOp.java | 6 ++-- .../ftc16072/OpModes/ScrimmageTeleop.java | 10 ++++++- .../ftc/teamcode/ftc16072/Robot.java | 14 ++++----- .../ftc16072/Util/PIDFController.java | 7 +++++ 6 files changed, 46 insertions(+), 31 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Arm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Arm.java index 46b9033..33e5dec 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Arm.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Arm.java @@ -1,32 +1,35 @@ package org.firstinspires.ftc.teamcode.ftc16072.Mechanisms; +import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.ftc16072.Tests.QQTest; import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestMotor; import org.firstinspires.ftc.teamcode.ftc16072.Util.PIDFController; import java.util.Arrays; import java.util.List; - +@Config public class Arm extends QQMechanism{ - public static final double TEST_SPEED = 0.55; - public static double TEST_SPEED_OFF = 0.4; + public static final double TEST_SPEED = 0.35; DcMotor armMotor; public static double kP = 0.01; public static double kI = 0.0; public static double kD = 0.0; - public static double kF = 0.4; - public static double max = 0.8; - public static double min = 0.0; + public static double kF = 0; + public static double max = 0.4; + public static double min = -max; public int currentPos; public int desiredPos; public double motorPower; - public int PLACEMENT_POSITION = 1000; //TODO make real - public int INTAKE_POSITION = 0; //TODO + public static int PLACEMENT_POSITION = 920; + public static int INTAKE_POSITION = 10; + + public Telemetry telemetry; PIDFController pidfController = new PIDFController(kP,kI,kD,kF,max,min); @@ -39,10 +42,7 @@ public void init(HardwareMap hwMap) { armMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); } - - public void setPosition(int desiredPos){ - this.desiredPos = desiredPos; - } + public void manualPositionChange(int changeAmount){ desiredPos += changeAmount; } @@ -58,6 +58,9 @@ public void goToPlacement(){ public void update(){ currentPos = (armMotor.getCurrentPosition()); double motorPower = pidfController.calculate(desiredPos,currentPos); + telemetry.addData("Current", currentPos); + telemetry.addData("Desired", desiredPos); + telemetry.addData("Motor Power", motorPower); this.motorPower = motorPower; armMotor.setPower(motorPower); } @@ -65,7 +68,8 @@ public void update(){ @Override public List getTests() { return Arrays.asList( - new TestMotor("arm move", TEST_SPEED, armMotor) + new TestMotor("arm up", TEST_SPEED, armMotor), + new TestMotor("arm down", -TEST_SPEED, armMotor) ); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Claw.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Claw.java index 2640c59..c8ce669 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Claw.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Claw.java @@ -1,18 +1,18 @@ package org.firstinspires.ftc.teamcode.ftc16072.Mechanisms; +import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.teamcode.ftc16072.Tests.QQTest; import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestServo; -import java.util.Arrays; import java.util.Collections; import java.util.List; - +@Config public class Claw extends QQMechanism { - public static double CLAW_OPEN_POSITION = 1; - public static double CLAW_CLOSE_POSITION = 0.7; + public static double CLAW_OPEN_POSITION = 0.3; + public static double CLAW_CLOSE_POSITION = 0; Servo clawServo; @@ -24,7 +24,7 @@ public void init(HardwareMap hwMap) { @Override public List getTests() { - return Arrays.asList( + return Collections.singletonList( new TestServo("claw_movement", CLAW_OPEN_POSITION, CLAW_CLOSE_POSITION, clawServo) ); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RI3WTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RI3WTeleOp.java index 89e0500..660bc33 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RI3WTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RI3WTeleOp.java @@ -2,8 +2,6 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.MecanumDrive; - @TeleOp @SuppressWarnings("unused") public class RI3WTeleOp extends QQOpMode{ @@ -14,7 +12,7 @@ public class RI3WTeleOp extends QQOpMode{ @Override public void loop() { - super.loop(); + /* super.loop(); double forward = -gamepad1.left_stick_y; double left = gamepad1.left_stick_x; double rotate = gamepad1.right_stick_x; @@ -56,6 +54,6 @@ else if (gamepad1.right_bumper){ robot.controlHub.resetGyro(); } - +*/ } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ScrimmageTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ScrimmageTeleop.java index 4b22c40..ba83afb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ScrimmageTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ScrimmageTeleop.java @@ -1,18 +1,26 @@ package org.firstinspires.ftc.teamcode.ftc16072.OpModes; -import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.MecanumDrive; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.MecanumDrive; +@TeleOp public class ScrimmageTeleop extends QQOpMode{ public static final double TRIGGER_THRESHOLD = 0.5; public static final int MANUAL_CHANGE = 5; + public void init(){ + super.init(); + robot.arm.telemetry = telemetry; + } public void loop(){ super.loop(); double forward = -gamepad1.left_stick_y; double left = gamepad1.left_stick_x; double rotate = gamepad1.right_stick_x; + nav.driveFieldRelative(forward, left, rotate); + if (gamepad1.left_trigger > TRIGGER_THRESHOLD ){ robot.mecanumDrive.setSpeed(MecanumDrive.Speed.TURBO); } else if (gamepad1.left_bumper) { 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 80118a9..c9e2616 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 @@ -5,11 +5,9 @@ import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Arm; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Claw; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.ControlHub; -import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.DoubleReverse4Bar; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.MecanumDrive; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.OpticalTrackingOdometrySensor; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.QQMechanism; -import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Slides; import java.util.Arrays; import java.util.List; @@ -20,8 +18,8 @@ public class Robot { public MecanumDrive mecanumDrive; public OpticalTrackingOdometrySensor otos; public Claw claw; - public DoubleReverse4Bar doubleReverse4Bar; - public Slides slides; + //public DoubleReverse4Bar doubleReverse4Bar; + // public Slides slides; public Arm arm; List mechanisms; @@ -30,8 +28,8 @@ public Robot() { controlHub = new ControlHub(); otos = new OpticalTrackingOdometrySensor(); claw = new Claw(); - doubleReverse4Bar = new DoubleReverse4Bar(); - slides = new Slides(); + //doubleReverse4Bar = new DoubleReverse4Bar(); + //slides = new Slides(); arm = new Arm(); mechanisms = Arrays.asList( @@ -39,9 +37,9 @@ public Robot() { mecanumDrive, otos, claw, - slides, + // slides, claw, - doubleReverse4Bar, + //doubleReverse4Bar, arm); } public void init(HardwareMap hwMap) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/PIDFController.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/PIDFController.java index 216f475..3315923 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/PIDFController.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/PIDFController.java @@ -12,6 +12,7 @@ public class PIDFController { double sumErrors; double lastError; + double lastDesired; ElapsedTime timer = new ElapsedTime(); public PIDFController(double kP,double kI,double kD,double kF, double max, double min){ @@ -28,6 +29,10 @@ public void reset(){ timer.reset(); } public double calculate(double desired, double current){ + if (lastDesired != desired){ + reset(); + } + double error = desired - current; double derivative = (error-lastError)/timer.seconds(); sumErrors += error * timer.seconds(); @@ -36,6 +41,8 @@ public double calculate(double desired, double current){ timer.reset(); lastError = error; + lastDesired = desired; + result = Math.min(max,result); result = Math.max(min,result); From c0225907a9c64638a0f67fc04f7ef43fc14a0d34 Mon Sep 17 00:00:00 2001 From: mahin Date: Mon, 14 Oct 2024 21:03:14 -0400 Subject: [PATCH 4/4] Added Gyroreset into ScrimTeleop and made slow drive slower --- .../ftc/teamcode/ftc16072/Mechanisms/MecanumDrive.java | 2 +- .../ftc/teamcode/ftc16072/OpModes/ScrimmageTeleop.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/MecanumDrive.java index 11f380e..5b91fb9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/MecanumDrive.java @@ -47,7 +47,7 @@ public void setSpeed(Speed speed) { speedMultiplier= 0.5; break; case SLOW : - speedMultiplier= 0.25; + speedMultiplier= 0.1; break; case FAST: speedMultiplier= 0.75; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ScrimmageTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ScrimmageTeleop.java index ba83afb..df510bd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ScrimmageTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ScrimmageTeleop.java @@ -35,9 +35,9 @@ else if (gamepad1.dpad_up){ robot.arm.manualPositionChange(MANUAL_CHANGE); }else if (gamepad1.dpad_down){ robot.arm.manualPositionChange(-MANUAL_CHANGE); + }else if (gamepad1.y){ + robot.controlHub.resetGyro(); } - - if (gamepad1.right_bumper) { robot.claw.close(); }else if (gamepad1.right_trigger > TRIGGER_THRESHOLD){