SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Need path tuning high basket auto path 1 (I need oranges)

Browse Source
This commit is contained in:
robotics1
2025-01-20 12:41:48 -08:00
parent 2a1513a2ba
commit e1be70c23f
11 changed files with 519 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java
View File

@ -46,7 +46,7 @@ public class PedroConstants {
// Robot encoder direction
public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD;
public static final double RIGHT_ENCODER_DIRECTION = Encoder.REVERSE;
public static final double BACK_ENCODER_DIRECTION = Encoder.FORWARD;
public static final double BACK_ENCODER_DIRECTION = Encoder.REVERSE;
// Arm config
public static final String LIFT_SLIDE_LEFT_MOTOR = "lift-slide-left";
@ -64,16 +64,16 @@ public class PedroConstants {
public static final double ROBOT_WEIGHT_IN_KG = 9;
// Maximum velocity of the robot going forward
public static final double ROBOT_SPEED_FORWARD = 53.223;
public static final double ROBOT_SPEED_FORWARD = 50.02;
// Maximum velocity of the robot going right
public static final double ROBOT_SPEED_LATERAL = 41.4081;
public static final double ROBOT_SPEED_LATERAL = 36.07;
// Rate of deceleration when power is cut-off when the robot is moving forward
public static final double FORWARD_ZERO_POWER_ACCEL = -76.8421;
public static final double FORWARD_ZERO_POWER_ACCEL = -91.4557;
// Rate of deceleration when power is cut-off when the robot is moving to the right
public static final double LATERAL_ZERO_POWER_ACCEL = -93.4183;
public static final double LATERAL_ZERO_POWER_ACCEL = -98.514;
// Determines how fast your robot will decelerate as a factor of how fast your robot will coast to a stop
public static final double ZERO_POWER_ACCEL_MULT = 4.0;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java
View File

@ -156,7 +156,7 @@ public class CometBotAutoDevelopment {
}
private void dualSlidesToFloorPosition() {
if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) {
dualSlides.toFloorPosotion();
dualSlides.toFloorPosition();
}
}
private void dualSlidesToLowBucketPosition() {

310
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java Normal file
View File

@ -0,0 +1,310 @@
package org.firstinspires.ftc.teamcode.cometbots.paths;
import static com.qualcomm.hardware.lynx.commands.core.LynxSetMotorPIDFControlLoopCoefficientsCommand.InternalMotorControlAlgorithm.Max;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.SleepAction;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.util.ElapsedTime;
//import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleopCompetition;
//import org.firstinspires.ftc.teamcode.cometbots.paths.HighBasketPath1;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
//import org.firstinspires.ftc.teamcode.subsystem.AutoPark;
import org.firstinspires.ftc.teamcode.cometbots.paths.HighBasketPath1;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath2;
//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath3;
//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath4;
//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath5;
//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath6;
//import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
//import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem;
@Autonomous(name = "Auto Test Competition V2", group = "Dev")
public class CometBotDriveV2 extends OpMode {
private Follower follower;
private int state;
private HighBasketPath1 path1;
//private CometBotTeleopCompetition comp;
private static ElapsedTime runtime;
private static boolean initalized = false;
private static boolean followingPath = false;
private DualMotorSliderSubsystem lift;
private ClawSubsystem claw;
private WristSubsystem wrist;
private ArmSubsystem arm;
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.75);
path1 = new HighBasketPath1();
lift = new DualMotorSliderSubsystem(hardwareMap);
arm = new ArmSubsystem(hardwareMap);
wrist = new WristSubsystem(hardwareMap);
claw = new ClawSubsystem(hardwareMap);
lift.init();
arm.initAuto();
wrist.initTeleOp();
claw.init();
//comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
// comp.initCloseClaw();
}
public void loop() {
telemetry.addData("state", state);
telemetry.addData("followingPath", followingPath);
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
}
switch (state) {
case 0:
moveToPathOneAndHighBucket();
// doArmThing1();
break;
case 1:
// doArmThing2();
break;
// case 2:
// moveToPathTwoAndPickSampleUp();
// break;
// case 3:
// doPickUpThing();
// break;
// case 4:
// moveToBasketPath3();
// break;
// case 5:
// theArmThing();
// break;
// case 6:
// moveToPickupAgainPath4();
// break;
// case 7:
// doPickUpThingAgain();
// break;
// case 8:
// moveToPickupAgainPath5();
// break;
// case 9:
// //theArmThingAgain();
// break;
// case 10:
// //moveToParkPath6();
// break;
// case 11:
}
telemetry.update();
follower.update();
}
private void moveToPathOneAndHighBucket() {
if (!followingPath) {
runtime = new ElapsedTime();
path1.moveToPath1(follower);
followingPath = true;
}
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
if (runtime.seconds() > 4) {
state = 1;
followingPath = false;
}
}
}
public class SetStateAction implements Action {
private int value;
public SetStateAction(int value) {
this.value = value;
}
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
state = value;
return false;
}
}
private Action setStateValue(int value) {
return new SetStateAction(value);
}
private void doArmThing1() {
lift.toHighBucketPosition();
arm.toBucketPosition();
state = 2;
}
private void doArmThing2() {
wrist.toBucketPosition();
claw.openClaw();
wrist.toFloorPosition();
state = 2;
}
// private void theArmThing() {
// telemetry.addData("busy?", follower.isBusy());
// telemetry.addData("end?", follower.atParametricEnd());
// if (follower.atParametricEnd()){
// follower.breakFollowing();
// comp.highBucketDropAuto();
// state = 6;
// }
// follower.breakFollowing();
// }
// private void theArmThingAgain() {
// follower.breakFollowing();
// comp.highBucketDropAuto();
// state = 10;
// }
// private void moveToPathTwoAndPickSampleUp() {
// if (!followingPath) {
// path2.moveToPath2(follower);
// followingPath = true;
// }
// if (runtime != null) {
// telemetry.addData("Runtime (seconds)", runtime.seconds());
// if (follower.atParametricEnd() || runtime.seconds() > 22.0) {
// state = 3;
// followingPath = false;
// }
// }
// }
//
// private void moveToPickupAgainPath4() {
// if (!followingPath) {
// path4.moveToPickupAgainPath4(follower);
// followingPath = true;
// }
// if (runtime != null) {
// telemetry.addData("Runtime (seconds)", runtime.seconds());
// if (follower.atParametricEnd() || runtime.seconds() > 27.0) {
// state = 7;
// followingPath = false;
// }
// }
// }
//
// private void moveToPickupAgainPath5() {
// if (!followingPath) {
// path5.moveToPickupAgainPath5(follower);
// followingPath = true;
// }
// if (runtime != null) {
// telemetry.addData("Runtime (seconds)", runtime.seconds());
// if (follower.atParametricEnd() || runtime.seconds() > 36.0) {
// state = 9;
// followingPath = false;
// }
// }
// }
// private void moveToParkPath6() {
// if (!followingPath) {
// path6.moveToParkPath6(follower);
// followingPath = true;
// }
// if (runtime != null) {
// telemetry.addData("Runtime (seconds)", runtime.seconds());
// if (follower.atParametricEnd() || runtime.seconds() > 48.0) {
// state = 11;
// followingPath = false;
// }
// }
// }
//
// private void moveToBasketPath3() {
// if (!followingPath) {
// path3.moveToBasketPath3(follower);
// followingPath = true;
// }
// if (runtime != null) {
// telemetry.addData("Runtime (seconds)", runtime.seconds());
// if (follower.atParametricEnd() || runtime.seconds() > 10.0) {
// state = 5;
// followingPath = false;
// }
// }
// }
//
// private void thePickUpAuto() {
// Actions.runBlocking(new SequentialAction(
// new SleepAction(.1),
// comp.claw.openClaw(),
// new SleepAction(.2),
// comp.wrist.toPickupPosition(),
// new SleepAction(.2),
// comp.arm.toSubmarinePosition(),
// new SleepAction(.5),
// comp.claw.closeClaw(),
// new SleepAction(.3),
// comp.wrist.toFloorPosition(),
// new SleepAction(.2),
// comp.arm.toParkPosition(),
// new SleepAction(.2)
// ));
// }
// private void thePickUp() {
// Actions.runBlocking(new SequentialAction(
// new SleepAction(.5),
// comp.wrist.toPickupPosition(),
// new SleepAction(.5),
// comp.arm.toSubmarinePosition(),
// new SleepAction(.7),
// comp.claw.closeClaw(),
// new SleepAction(.7),
// comp.wrist.toFloorPosition(),
// new SleepAction(.5),
// comp.arm.toParkPosition(),
// new SleepAction(.5)
// ));
// }
//
// private void doPickUpThing() {
// follower.breakFollowing();
// thePickUpAuto();
// state = 4;
// }
//
// private void doPickUpThingAgain() {
// follower.breakFollowing();
// thePickUpAuto();
// state = 8;
// }
//
//
}

38
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath1.java Normal file
View File

@ -0,0 +1,38 @@
package org.firstinspires.ftc.teamcode.cometbots.paths;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/*
AutoLine# - This file does something of a path......
*/
public class HighBasketPath1 {
private final Pose startPose = new Pose(8, 89);
public void moveToPath1(Follower robot) {
PathChain pathChain;
robot.setStartingPose(startPose);
PathBuilder builder = new PathBuilder();
builder
.addPath(
new BezierCurve(
new Point(8.000, 89.000, Point.CARTESIAN),
new Point(24.000, 96.000, Point.CARTESIAN),
new Point(18.000, 126.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
pathChain = builder.build();
robot.followPath(pathChain);
}
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java
View File

@ -56,7 +56,7 @@ public class ArmTest extends LinearOpMode {
Gamepad currentGamepad1 = new Gamepad();
Gamepad previousGamepad1 = new Gamepad();
arm.init();
arm.initTeleOp();
waitForStart();
runtime.reset();

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/WristTest.java
View File

@ -57,7 +57,7 @@ public class WristTest extends LinearOpMode {
Gamepad currentGamepad1 = new Gamepad();
Gamepad previousGamepad1 = new Gamepad();
wrist.init();
wrist.initTeleOp();
waitForStart();
runtime.reset();

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java
View File

@ -4,7 +4,7 @@ import com.acmerobotics.dashboard.config.Config;
@Config
public class RobotConstants {
public final static double clawClose = 0.85;
public final static double clawClose = 0.95;
public final static double clawOpen = 0.05;
public final static double armFloor = 0.7;
@ -19,9 +19,9 @@ public class RobotConstants {
public final static double armBucket = 0.45;
public final static double armInit = 0.125;
public final static double wristInit = 0.0;
public final static double wristPickup = 0.45;
public final static double wristBucket = 0.6;
public final static double wristFloor = 0.85;
public final static double wristPickup = 0.475;
public final static double wristBucket = 0.56;
public final static double wristFloor = 0.75;
public final static double wristRung = 0.55;
@ -30,13 +30,13 @@ public class RobotConstants {
public final static int liftToFloorPos = 350;
public final static int liftToSubmarinePos = 350;
public final static int liftToLowBucketPos = 2650;
public final static int liftToLowBucketPos = 1750;
public final static int liftToHighRung = 2100;
public final static int dropToHighRung = 1675;
public final static int liftToHighRungAttach = 1050;
public final static int liftToHighBucketPos = 4900;
public final static int liftToHighBucketPos = 4000;
public final static double liftPower = 1;
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java
View File

@ -31,7 +31,7 @@ public class FollowerConstants {
public static String rightFrontMotorName = FRONT_RIGHT_MOTOR;
public static String rightRearMotorName = BACK_RIGHT_MOTOR;
public static DcMotorSimple.Direction leftFrontMotorDirection = FRONT_RIGHT_MOTOR_DIRECTION;
public static DcMotorSimple.Direction leftFrontMotorDirection = FRONT_LEFT_MOTOR_DIRECTION;
public static DcMotorSimple.Direction rightFrontMotorDirection = FRONT_RIGHT_MOTOR_DIRECTION;
public static DcMotorSimple.Direction leftRearMotorDirection = BACK_LEFT_MOTOR_DIRECTION;
public static DcMotorSimple.Direction rightRearMotorDirection = BACK_RIGHT_MOTOR_DIRECTION;
@ -48,7 +48,7 @@ public class FollowerConstants {
public static CustomPIDFCoefficients translationalPIDFCoefficients = new CustomPIDFCoefficients(
.25,
0,
0.0375,
0.02,
0);
// Translational Integral
@ -75,7 +75,7 @@ public class FollowerConstants {
// Drive PIDF coefficients
public static CustomFilteredPIDFCoefficients drivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
0.00375,
0.00235,
0,
0.00003,
0.8,

13
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java
View File

@ -16,7 +16,7 @@ public class ClawSubsystem {
}
public enum ThumbState {
DOWN, UP
DOWN, UP, AWAY
}
private Servo claw;
@ -34,11 +34,13 @@ public class ClawSubsystem {
}
public void closeClaw() {
claw.setPosition(clawClose);
thumbDown();
state = ClawState.CLOSED;
}
public void openClaw() {
claw.setPosition(clawOpen);
thumbUp();
state = ClawState.OPEN;
}
@ -72,13 +74,18 @@ public class ClawSubsystem {
}
public void thumbUp() {
thumb.setPosition(0.95);
thumb.setPosition(0.5);
tState = ThumbState.UP;
}
public void thumbDown() {
thumb.setPosition(0.05);
thumb.setPosition(0.00);
tState = ThumbState.DOWN;
}
public void thumbOutOfTheWay() {
thumb.setPosition(0.95);
tState = ThumbState.AWAY;
}
}

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java
View File

@ -2,6 +2,8 @@ package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
@ -123,13 +125,21 @@ public class DualMotorSliderSubsystem {
}
public void toLowBucketPosition() {
setTargetPosition(2000);
setTargetPosition(liftToLowBucketPos);
}
public void toHighBucketPosition() {
setTargetPosition(4000);
setTargetPosition(liftToHighBucketPos);
}
public void toFloorPosotion(){setTargetPosition(0);}
public void toFixedPosition(int value) {
setTargetPosition(value);
}
public int getFixedPosition() {
return liftSlideLeft.getCurrentPosition();
}
public void toFloorPosition(){setTargetPosition(0);}
}

131
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java Normal file
View File

@ -0,0 +1,131 @@
package org.firstinspires.ftc.teamcode.subsystem;
/* Copyright (c) 2021 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.SleepAction;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
@TeleOp(name = "Lift Wrist Arm Test", group = "Debug")
public class LiftArmWrist extends LinearOpMode {
private final ElapsedTime runtime = new ElapsedTime();
@Override
public void runOpMode() {
/*
* Instantiate Lift
*/
DualMotorSliderSubsystem lift = new DualMotorSliderSubsystem(hardwareMap);
WristSubsystem wrist = new WristSubsystem(hardwareMap);
ArmSubsystem arm = new ArmSubsystem(hardwareMap);
ClawSubsystem claw = new ClawSubsystem(hardwareMap);
/*
* Instantiate gamepad state holders
*/
Gamepad currentGamepad1 = new Gamepad();
Gamepad previousGamepad1 = new Gamepad();
lift.init();
wrist.initTeleOp();
arm.initTeleOp();
claw.init();
waitForStart();
runtime.reset();
// run until the end of the match (driver presses STOP)
while (opModeIsActive()) {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
if (currentGamepad1.dpad_up && !previousGamepad1.dpad_up) {
arm.setPosition(arm.getPosition() + .05);
}
if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) {
arm.setPosition(arm.getPosition() - .05);
}
if (currentGamepad1.b && !previousGamepad1.b) {
claw.switchState();
}
if (currentGamepad1.dpad_right && !previousGamepad1.dpad_right) {
lift.toHighBucketPosition();
}
if (currentGamepad1.dpad_left && !previousGamepad1.dpad_left) {
lift.toLowBucketPosition();
}
if (currentGamepad1.y && !previousGamepad1.y) {
wrist.setPosition(wrist.getPosition() + .05);
}
if (currentGamepad1.a && !previousGamepad1.a) {
wrist.setPosition(wrist.getPosition() - .05);
}
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
lift.toHighBucketPosition();
}
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
lift.toLowBucketPosition();
}
lift.update();
// Show the elapsed game time and wheel power.
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Lift Drive Position", lift.getFixedPosition());
telemetry.addData("Wrist Position", wrist.getPosition());
telemetry.addData("Arm Position", arm.getPosition());
telemetry.update();
}
}
}