Skip to content

Commit

Permalink
pid-tuning-arm-20403-kepler
Browse files Browse the repository at this point in the history
  • Loading branch information
@kevinfrei
Festivepotatoes authored and kevinfrei committed Nov 3, 2024
1 parent 8ad613d commit ef705c8
Show file tree
Hide file tree
Showing 8 changed files with 235 additions and 109 deletions.
1 change: 0 additions & 1 deletion LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/tele/AnalogMotorTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,6 @@
import org.firstinspires.ftc.learnbot.Setup;
import org.firstinspires.ftc.learnbot.controllers.MotorController;

@Disabled
@TeleOp(name = "AnalogMotorTest")
@SuppressWarnings("unused")
public class AnalogMotorTest extends CommandOpMode implements Loggable {
Expand Down
3 changes: 3 additions & 0 deletions Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Hardware.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
package org.firstinspires.ftc.twenty403;

import com.qualcomm.hardware.digitalchickenlabs.OctoQuad;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotorEx;
Expand Down Expand Up @@ -32,6 +33,7 @@ public class Hardware implements Loggable {
public Rev2MDistanceSensor rev2MDistanceSensor;
public Motor suspend;
public EncodedMotor<DcMotorEx> rotate1, rotate2, slides;
public OctoQuad octoquad;

/* Put other hardware here! */

Expand Down Expand Up @@ -67,6 +69,7 @@ public Hardware(HardwareMap hwmap) {
rotate1 = new EncodedMotor<>(Setup.HardwareNames.ARML);
rotate2 = new EncodedMotor<>(Setup.HardwareNames.ARMR);
slides = new EncodedMotor<>(Setup.HardwareNames.SLIDEMOTOR);
octoquad = hwmap.get(OctoQuad.class, Setup.HardwareNames.OCTOQUAD);
}
}

Expand Down
29 changes: 19 additions & 10 deletions Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Robot.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
package org.firstinspires.ftc.twenty403;

import com.qualcomm.hardware.digitalchickenlabs.OctoQuad;
import com.technototes.library.logger.Loggable;
import com.technototes.library.util.Alliance;
import org.firstinspires.ftc.twenty403.helpers.StartingPosition;
Expand All @@ -10,7 +11,7 @@
import org.firstinspires.ftc.twenty403.subsystems.SafetySubsystem;
import org.firstinspires.ftc.twenty403.subsystems.TwoDeadWheelLocalizer;

public class Robot implements Loggable {
public class Robot implements Loggable {

public StartingPosition position;
public Alliance alliance;
Expand All @@ -33,15 +34,23 @@ public Robot(Hardware hw, Alliance team, StartingPosition pos) {
this.localizer = null;
}
if (Setup.Connected.DRIVEBASE) {
this.drivebaseSubsystem = new DrivebaseSubsystem(
hw.fl,
hw.fr,
hw.rl,
hw.rr,
hw.imu,
localizer
);
if (localizer != null) {
if (localizer == null) {
this.drivebaseSubsystem = new DrivebaseSubsystem(
hw.fl,
hw.fr,
hw.rl,
hw.rr,
hw.imu
);
} else {
this.drivebaseSubsystem = new DrivebaseSubsystem(
hw.fl,
hw.fr,
hw.rl,
hw.rr,
hw.imu,
localizer
);
// YOU MUST CALL THIS IMMEDIATELY AFTER CREATING THE DRIVEBASE!
localizer.setDrivebase(this.drivebaseSubsystem);
}
Expand Down
15 changes: 12 additions & 3 deletions Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Setup.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,9 +10,9 @@ public static class Connected {
public static boolean DRIVEBASE = true;
public static boolean ODOSUBSYSTEM = false;
public static boolean SAFETYSUBSYSTEM = false;
public static boolean KIDSSHAMPOOSUBSYSTEM = true;
public static boolean HANGSUBSYSTEM = true;
public static boolean ARMSUBSYSTEM = false;
public static boolean KIDSSHAMPOOSUBSYSTEM = false;
public static boolean HANGSUBSYSTEM = false;
public static boolean ARMSUBSYSTEM = true;
}

@Config
Expand All @@ -23,6 +23,7 @@ public static class HardwareNames {
public static String RLMOTOR = "rl";
public static String RRMOTOR = "rr";
public static String IMU = "imu";
public static String OCTOQUAD = "octoquad";
public static String ODOF = "odof";
public static String ODOR = "odor";
public static String INTAKE = "intake";
Expand All @@ -37,6 +38,14 @@ public static class HardwareNames {
public static String SLIDEMOTOR = "slide";
}

@Config
public static class OctoQuadPorts {

public static int ARMENCODER = 5;
public static int ODOR = 6; //TODO: verify with robot, r & l may be swapped
public static int ODOL = 7;
}

@Config
public static class OtherSettings {

Expand Down
69 changes: 51 additions & 18 deletions Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/OperatorController.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,10 +23,10 @@ public class OperatorController {
public CommandButton stopIntake;
public CommandButton suspend;
public CommandButton SuspendReverse;
public CommandButton lowBasket;
public CommandButton highBasket;
public CommandButton lowSpecimen;
public CommandButton highSpecimen;
public CommandButton armIntake;
public CommandButton armLowNet;
public CommandButton armLowSpecimen;
public CommandButton armHighSpecimen;

public OperatorController(CommandGamepad g, Robot r) {
robot = r;
Expand All @@ -45,36 +45,69 @@ private void AssignNamedControllerButton() {
spitIntake = gamepad.rightBumper;
biteJaw = gamepad.ps_cross;
releaseJaw = gamepad.ps_triangle;
suspend = gamepad.dpadLeft;
suspend = gamepad.ps_circle;
armIntake = gamepad.ps_square;
armLowNet = gamepad.dpadLeft;
armLowSpecimen = gamepad.leftStickButton;
armHighSpecimen = gamepad.rightStickButton;
}

public void BindControls() {
if (Setup.Connected.KIDSSHAMPOOSUBSYSTEM){
if (Setup.Connected.KIDSSHAMPOOSUBSYSTEM) {
bindKidShampooControls();
}
if (Setup.Connected.HANGSUBSYSTEM){
if (Setup.Connected.HANGSUBSYSTEM) {
bindHangControls();
}

if (Setup.Connected.ARMSUBSYSTEM) {
bindArmControls();
}
}

public void bindKidShampooControls() {
openRetainer.whenPressed(KidShampooCmds.cmds.OpenRetainer(robot.kidShampooSubsystem));
closeRetainer.whenPressed(KidShampooCmds.cmds.CloseRetainer(robot.kidShampooSubsystem));
// eatRetainer.whenPressed(KidShampooCmds.cmds.EatRetainer(robot.kidShampooSubsystem));
biteJaw.whenPressed(KidShampooCmds.cmds.BiteJaw(robot.kidShampooSubsystem));
releaseJaw.whenPressed(KidShampooCmds.cmds.ReleaseJaw(robot.kidShampooSubsystem));
slurpIntake.whenPressed(KidShampooCmds.cmds.SlurpIntake(robot.kidShampooSubsystem));
spitIntake.whenPressed(KidShampooCmds.cmds.SpitIntake(robot.kidShampooSubsystem));
slurpIntake.whenReleased(KidShampooCmds.cmds.StopIntake(robot.kidShampooSubsystem));
spitIntake.whenReleased(KidShampooCmds.cmds.StopIntake(robot.kidShampooSubsystem));
openRetainer.whenPressed(
Command.create(robot.kidShampooSubsystem::openRetainer, robot.kidShampooSubsystem)
);
closeRetainer.whenPressed(
Command.create(robot.kidShampooSubsystem::closeRetainer, robot.kidShampooSubsystem)
);
eatRetainer.whenPressed(
Command.create(robot.kidShampooSubsystem::eatRetainer, robot.kidShampooSubsystem)
);
biteJaw.whenPressed(
Command.create(robot.kidShampooSubsystem::biteJaw, robot.kidShampooSubsystem)
);
releaseJaw.whenPressed(
Command.create(robot.kidShampooSubsystem::releaseJaw, robot.kidShampooSubsystem)
);
slurpIntake.whenPressed(
Command.create(robot.kidShampooSubsystem::slurpIntake, robot.kidShampooSubsystem)
);
spitIntake.whenPressed(
Command.create(robot.kidShampooSubsystem::spitIntake, robot.kidShampooSubsystem)
);
slurpIntake.whenReleased(
Command.create(robot.kidShampooSubsystem::stopIntake, robot.kidShampooSubsystem)
);
spitIntake.whenReleased(
Command.create(robot.kidShampooSubsystem::stopIntake, robot.kidShampooSubsystem)
);
}
public void bindArmControls() {
//lowBasket.whenPressed(new LowBasketCommand.LowBasket(robot));

public void bindArmControls() {
armIntake.whenPressed(
Command.create(robot.armSubsystem::setArmToIntake, robot.armSubsystem)
);
armLowNet.whenPressed(Command.create(robot.armSubsystem::lowBasket, robot.armSubsystem));
armLowSpecimen.whenPressed(
Command.create(robot.armSubsystem::lowSpecimen, robot.armSubsystem)
);
armHighSpecimen.whenPressed(
Command.create(robot.armSubsystem::highSpecimen, robot.armSubsystem)
);
}

public void bindHangControls() {
suspend.whenPressed(HangCmd.hang.Suspend(robot.hangSubsystem));
}
Expand Down
16 changes: 7 additions & 9 deletions Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmodes/auto/SplineTest1.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,6 @@
import com.technototes.library.command.SequentialCommandGroup;
import com.technototes.library.structure.CommandOpMode;
import com.technototes.library.util.Alliance;

import org.firstinspires.ftc.twenty403.AutoConstants;
import org.firstinspires.ftc.twenty403.Hardware;
import org.firstinspires.ftc.twenty403.Robot;
Expand All @@ -33,21 +32,20 @@ public void uponInit() {
hardware = new Hardware(hardwareMap);
robot = new Robot(hardware, Alliance.RED, StartingPosition.Wing);
robot.drivebaseSubsystem.setPoseEstimate(AutoConstants.BACKWARD.toPose());
// safety = new SafetyTestController(driverGamepad, robot);
// safety = new SafetyTestController(driverGamepad, robot);
//robot.safetySubsystem.startMonitoring();
CommandScheduler.scheduleForState(
new SequentialCommandGroup(
Paths.splineTestCommand(robot),
Paths.pushPathSpline(robot),
Paths.splineTestCommand(robot),
Paths.pushPathSpline(robot),
EZCmd.Drive.RecordHeading(robot.drivebaseSubsystem),
CommandScheduler::terminateOpMode
),
OpModeState.RUN
);

// CommandScheduler.scheduleForState(
// new SafetyStartCommand(robot.safetySubsystem),
// OpModeState.RUN
// );
// CommandScheduler.scheduleForState(
// new SafetyStartCommand(robot.safetySubsystem),
// OpModeState.RUN
// );
}
}
Loading

0 comments on commit ef705c8

Please sign in to comment.