Need path tuning high basket auto path 1 (I need oranges)
This commit is contained in:
@ -46,7 +46,7 @@ public class PedroConstants {
|
|||||||
// Robot encoder direction
|
// Robot encoder direction
|
||||||
public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD;
|
public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD;
|
||||||
public static final double RIGHT_ENCODER_DIRECTION = Encoder.REVERSE;
|
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
|
// Arm config
|
||||||
public static final String LIFT_SLIDE_LEFT_MOTOR = "lift-slide-left";
|
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;
|
public static final double ROBOT_WEIGHT_IN_KG = 9;
|
||||||
|
|
||||||
// Maximum velocity of the robot going forward
|
// 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
|
// 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
|
// 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
|
// 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
|
// 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;
|
public static final double ZERO_POWER_ACCEL_MULT = 4.0;
|
||||||
|
@ -156,7 +156,7 @@ public class CometBotAutoDevelopment {
|
|||||||
}
|
}
|
||||||
private void dualSlidesToFloorPosition() {
|
private void dualSlidesToFloorPosition() {
|
||||||
if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) {
|
if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) {
|
||||||
dualSlides.toFloorPosotion();
|
dualSlides.toFloorPosition();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
private void dualSlidesToLowBucketPosition() {
|
private void dualSlidesToLowBucketPosition() {
|
||||||
|
@ -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;
|
||||||
|
// }
|
||||||
|
//
|
||||||
|
//
|
||||||
|
}
|
||||||
|
|
@ -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);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
@ -56,7 +56,7 @@ public class ArmTest extends LinearOpMode {
|
|||||||
Gamepad currentGamepad1 = new Gamepad();
|
Gamepad currentGamepad1 = new Gamepad();
|
||||||
Gamepad previousGamepad1 = new Gamepad();
|
Gamepad previousGamepad1 = new Gamepad();
|
||||||
|
|
||||||
arm.init();
|
arm.initTeleOp();
|
||||||
waitForStart();
|
waitForStart();
|
||||||
runtime.reset();
|
runtime.reset();
|
||||||
|
|
||||||
|
@ -57,7 +57,7 @@ public class WristTest extends LinearOpMode {
|
|||||||
Gamepad currentGamepad1 = new Gamepad();
|
Gamepad currentGamepad1 = new Gamepad();
|
||||||
Gamepad previousGamepad1 = new Gamepad();
|
Gamepad previousGamepad1 = new Gamepad();
|
||||||
|
|
||||||
wrist.init();
|
wrist.initTeleOp();
|
||||||
waitForStart();
|
waitForStart();
|
||||||
runtime.reset();
|
runtime.reset();
|
||||||
|
|
||||||
|
@ -4,7 +4,7 @@ import com.acmerobotics.dashboard.config.Config;
|
|||||||
|
|
||||||
@Config
|
@Config
|
||||||
public class RobotConstants {
|
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 clawOpen = 0.05;
|
||||||
|
|
||||||
public final static double armFloor = 0.7;
|
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 armBucket = 0.45;
|
||||||
public final static double armInit = 0.125;
|
public final static double armInit = 0.125;
|
||||||
public final static double wristInit = 0.0;
|
public final static double wristInit = 0.0;
|
||||||
public final static double wristPickup = 0.45;
|
public final static double wristPickup = 0.475;
|
||||||
public final static double wristBucket = 0.6;
|
public final static double wristBucket = 0.56;
|
||||||
public final static double wristFloor = 0.85;
|
public final static double wristFloor = 0.75;
|
||||||
|
|
||||||
public final static double wristRung = 0.55;
|
public final static double wristRung = 0.55;
|
||||||
|
|
||||||
@ -30,13 +30,13 @@ public class RobotConstants {
|
|||||||
|
|
||||||
public final static int liftToFloorPos = 350;
|
public final static int liftToFloorPos = 350;
|
||||||
public final static int liftToSubmarinePos = 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 liftToHighRung = 2100;
|
||||||
public final static int dropToHighRung = 1675;
|
public final static int dropToHighRung = 1675;
|
||||||
|
|
||||||
public final static int liftToHighRungAttach = 1050;
|
public final static int liftToHighRungAttach = 1050;
|
||||||
|
|
||||||
|
|
||||||
public final static int liftToHighBucketPos = 4900;
|
public final static int liftToHighBucketPos = 4000;
|
||||||
public final static double liftPower = 1;
|
public final static double liftPower = 1;
|
||||||
}
|
}
|
@ -31,7 +31,7 @@ public class FollowerConstants {
|
|||||||
public static String rightFrontMotorName = FRONT_RIGHT_MOTOR;
|
public static String rightFrontMotorName = FRONT_RIGHT_MOTOR;
|
||||||
public static String rightRearMotorName = BACK_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 rightFrontMotorDirection = FRONT_RIGHT_MOTOR_DIRECTION;
|
||||||
public static DcMotorSimple.Direction leftRearMotorDirection = BACK_LEFT_MOTOR_DIRECTION;
|
public static DcMotorSimple.Direction leftRearMotorDirection = BACK_LEFT_MOTOR_DIRECTION;
|
||||||
public static DcMotorSimple.Direction rightRearMotorDirection = BACK_RIGHT_MOTOR_DIRECTION;
|
public static DcMotorSimple.Direction rightRearMotorDirection = BACK_RIGHT_MOTOR_DIRECTION;
|
||||||
@ -48,7 +48,7 @@ public class FollowerConstants {
|
|||||||
public static CustomPIDFCoefficients translationalPIDFCoefficients = new CustomPIDFCoefficients(
|
public static CustomPIDFCoefficients translationalPIDFCoefficients = new CustomPIDFCoefficients(
|
||||||
.25,
|
.25,
|
||||||
0,
|
0,
|
||||||
0.0375,
|
0.02,
|
||||||
0);
|
0);
|
||||||
|
|
||||||
// Translational Integral
|
// Translational Integral
|
||||||
@ -75,7 +75,7 @@ public class FollowerConstants {
|
|||||||
|
|
||||||
// Drive PIDF coefficients
|
// Drive PIDF coefficients
|
||||||
public static CustomFilteredPIDFCoefficients drivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
|
public static CustomFilteredPIDFCoefficients drivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
|
||||||
0.00375,
|
0.00235,
|
||||||
0,
|
0,
|
||||||
0.00003,
|
0.00003,
|
||||||
0.8,
|
0.8,
|
||||||
|
@ -16,7 +16,7 @@ public class ClawSubsystem {
|
|||||||
}
|
}
|
||||||
|
|
||||||
public enum ThumbState {
|
public enum ThumbState {
|
||||||
DOWN, UP
|
DOWN, UP, AWAY
|
||||||
}
|
}
|
||||||
|
|
||||||
private Servo claw;
|
private Servo claw;
|
||||||
@ -34,11 +34,13 @@ public class ClawSubsystem {
|
|||||||
}
|
}
|
||||||
public void closeClaw() {
|
public void closeClaw() {
|
||||||
claw.setPosition(clawClose);
|
claw.setPosition(clawClose);
|
||||||
|
thumbDown();
|
||||||
state = ClawState.CLOSED;
|
state = ClawState.CLOSED;
|
||||||
}
|
}
|
||||||
|
|
||||||
public void openClaw() {
|
public void openClaw() {
|
||||||
claw.setPosition(clawOpen);
|
claw.setPosition(clawOpen);
|
||||||
|
thumbUp();
|
||||||
state = ClawState.OPEN;
|
state = ClawState.OPEN;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -72,13 +74,18 @@ public class ClawSubsystem {
|
|||||||
}
|
}
|
||||||
|
|
||||||
public void thumbUp() {
|
public void thumbUp() {
|
||||||
thumb.setPosition(0.95);
|
thumb.setPosition(0.5);
|
||||||
tState = ThumbState.UP;
|
tState = ThumbState.UP;
|
||||||
}
|
}
|
||||||
|
|
||||||
public void thumbDown() {
|
public void thumbDown() {
|
||||||
thumb.setPosition(0.05);
|
thumb.setPosition(0.00);
|
||||||
tState = ThumbState.DOWN;
|
tState = ThumbState.DOWN;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public void thumbOutOfTheWay() {
|
||||||
|
thumb.setPosition(0.95);
|
||||||
|
tState = ThumbState.AWAY;
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
@ -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_LEFT_MOTOR;
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_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.DcMotor;
|
||||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||||
@ -123,13 +125,21 @@ public class DualMotorSliderSubsystem {
|
|||||||
}
|
}
|
||||||
|
|
||||||
public void toLowBucketPosition() {
|
public void toLowBucketPosition() {
|
||||||
setTargetPosition(2000);
|
setTargetPosition(liftToLowBucketPos);
|
||||||
}
|
}
|
||||||
|
|
||||||
public void toHighBucketPosition() {
|
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);}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
@ -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();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
Reference in New Issue
Block a user