Skip to content

Commit

Permalink
This is the initial 16750 vertical slides stuff (#77)
Browse files Browse the repository at this point in the history 
* more work on vertical slides, haven't gotten it working but we set up feedfwd finally

* vertical slides testing - got the slides moving, values are somewhat accurate. made an up and down feedfwd and haven't tested down. still need to test high basket, no testing done on chamber pos yet. commented out a lot of other buttons for now, may need to remove some excess code at some point. might want to switch the slides to the joysticks at some point, not sure. still need to figure out how to merge this controller with the other one. PID for slides should be good now. added an if statement for horizontal subsystem related things on both dualtele and verttest opmodes, they work

* vertical slides testing - got the slides moving, values are somewhat accurate. made an up and down feedfwd and haven't tested down. still need to test high basket, no testing done on chamber pos yet. commented out a lot of other buttons for now, may need to remove some excess code at some point. might want to switch the slides to the joysticks at some point, not sure. still need to figure out how to merge this controller with the other one. PID for slides should be good now. added an if statement for horizontal subsystem related things on both dualtele and verttest opmodes, they work

* hopefully figured out the operator controller command switch thingy. added an if connected for upon start for vertical subsystem
carry forward:
-test feedfwd down test high basket -uncomment and make choice commands -remove excess code for legibility -manual slides on joysticks?

* Added a missing method that I think got eaten during a merge or rebase

* Got the Frog Princess code deployed to the bot. Needs PID tuned, but looks to be doing the right thing

---------

Co-authored-by: Braelyn <[email protected]>
  • Loading branch information
@kevinfrei @braelynandthefrogs
kevinfrei and braelynandthefrogs authored Nov 16, 2024
1 parent 0f346e6 commit 1132143
Show file tree
Hide file tree
Showing 6 changed files with 102 additions and 20 deletions.
10 changes: 8 additions & 2 deletions ...rc/main/java/org/firstinspires/ftc/sixteen750/commands/slides/VerticalSlidesCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,10 +55,16 @@ public static Command ArmTransfer(Robot r) {
public static Command ArmScore(Robot r) {
return Command.create(r.verticalSlidesSubsystem::armServoEmpty, r.verticalSlidesSubsystem);
}

public static Command SlidesZero(Robot r) {
public static Command SlidesUp(Robot r) {
return Command.create(r.verticalSlidesSubsystem::slidesUp, r.verticalSlidesSubsystem);
}
public static Command SlidesDown(Robot r) {
return Command.create(r.verticalSlidesSubsystem::slidesDown, r.verticalSlidesSubsystem);
}
public static Command SlidesZero(Robot r) {
return Command.create(r.verticalSlidesSubsystem::resetSlideZero, r.verticalSlidesSubsystem);
}

//transfer
//inc/dec

Expand Down
64 changes: 62 additions & 2 deletions Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/OperatorController.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,12 +9,16 @@
import org.firstinspires.ftc.sixteen750.commands.driving.JoystickDriveCommand;
import org.firstinspires.ftc.sixteen750.commands.slides.HorizontalAnalogCommand;
import org.firstinspires.ftc.sixteen750.commands.slides.HorizontalSlidesCommands;
import org.firstinspires.ftc.sixteen750.commands.slides.VerticalSlidesCommands;

public class OperatorController {

public Robot robot;
public Stick horislidesLeftStick;
public CommandGamepad gamepad;
public CommandButton shiftButton;

//horizontal buttons
public Stick horislidesLeftStick;
public CommandButton openClaw;
public CommandButton closeClaw;
public CommandButton wristPickup;
Expand All @@ -23,6 +27,36 @@ public class OperatorController {
public CommandButton wristDecrement;
public CommandButton horislidesExtend;
public CommandButton horislidesRetract;
//vertical buttons
public Stick vertslidesLeftStick;
public CommandButton bucketTransfer;
public CommandButton bucketScore;
public CommandButton bucketIncrement;
public CommandButton bucketDecrement;
public CommandButton armTransfer;
public CommandButton armScore;
public CommandButton slidesLow;
public CommandButton slidesHigh;
public CommandButton slidesZero;
public CommandButton slidesDown;
//public CommandButton wristDecrement;

//can we make a command happen when we switch modes? like horizontal retract when vertical is active vice versa
//can make sequential on the button to retract slides when shifting

//choice commands
public CommandButton horizontalSlides_verticalSlides;
public boolean shifted = false;

public boolean isShifted() {
return shifted;
}
public boolean notShifted() {
return !shifted;
}
public void toggleShift() {
shifted = !shifted;
}

public OperatorController(CommandGamepad g, Robot r) {
robot = r;
Expand All @@ -41,6 +75,14 @@ private void AssignNamedControllerButton() {
horislidesLeftStick = gamepad.leftStick;
horislidesExtend = gamepad.ps_cross;
horislidesRetract = gamepad.ps_square;

//choice commands
shiftButton = gamepad.ps_share;
horizontalSlides_verticalSlides = gamepad.ps_triangle;
slidesHigh = gamepad.dpadUp;
slidesLow = gamepad.dpadDown;
slidesDown = gamepad.rightStickButton;
slidesZero = gamepad.ps_options;
}

private void BindButtons() {
Expand All @@ -49,8 +91,13 @@ private void BindButtons() {
bindHorizontalSlidesControls();
bindHorizontalAnalogControls();
}
}

if (Setup.Connected.VERTICALSLIDESUBSYSTEM) {
bindVerticalSlidesControls();
}
shiftButton.whenPressed(this::toggleShift);
}
//can we toggle between regular buttons ex:open/close claw as one button - kevin will work on that
private void bindHorizontalSlidesControls() {
openClaw.whenPressed(HorizontalSlidesCommands.clawOpen(robot));
closeClaw.whenPressed(HorizontalSlidesCommands.clawChomp(robot));
Expand All @@ -61,6 +108,19 @@ private void bindHorizontalSlidesControls() {
horislidesExtend.whenPressed(HorizontalSlidesCommands.intake(robot));
horislidesRetract.whenPressed(HorizontalSlidesCommands.transferring(robot));
}
private void bindVerticalSlidesControls() {
// bucketTransfer.whenPressed(VerticalSlidesCommands.BucketTransfer(robot));
// bucketScore.whenPressed(VerticalSlidesCommands.BucketEmpty(robot));
// armTransfer.whenPressed(VerticalSlidesCommands.ArmTransfer(robot));
// armScore.whenPressed(VerticalSlidesCommands.ArmScore(robot));
slidesHigh.whenPressed(VerticalSlidesCommands.HighBasket(robot));
slidesLow.whenPressed(VerticalSlidesCommands.LowBasket(robot));
slidesDown.whenPressed(VerticalSlidesCommands.SlidesDown(robot));
slidesZero.whenPressed(VerticalSlidesCommands.SlidesZero(robot));
//wristDecrement.whenPressed(SlidesCommands.wristDecrement(robot));
// slidesUpTesting.whenPressed(VerticalSlidesCommands.SlidesUp(robot));
// slidesDownTesting.whenPressed(VerticalSlidesCommands.SlidesDown(robot));
}

public void bindHorizontalAnalogControls() {
CommandScheduler.scheduleJoystick(
Expand Down
9 changes: 5 additions & 4 deletions ...0/src/main/java/org/firstinspires/ftc/sixteen750/controls/OperatorControllerVertical.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@ public class OperatorControllerVertical {
public CommandButton bucketTransfer;
public CommandButton bucketScore;
public CommandButton bucketIncrement;
public CommandButton bucketDecrement;
public CommandButton armTransfer;
public CommandButton armScore;
public CommandButton slidesLow;
Expand All @@ -33,13 +34,13 @@ public OperatorControllerVertical(CommandGamepad g, Robot r) {
private void AssignNamedControllerButton() {
bucketTransfer = gamepad.ps_circle;
bucketScore = gamepad.ps_triangle;
bucketIncrement =
armTransfer = gamepad.ps_cross;
armScore = gamepad.ps_square;
slidesLow = gamepad.dpadLeft;
slidesHigh = gamepad.dpadUp;
slidesHigh = gamepad.ps_share;
slidesLow = gamepad.dpadUp;
slidesDown = gamepad.dpadDown;
//wristDecrement = gamepad.dpadDown;
bucketIncrement = gamepad.dpadDown;
bucketDecrement = gamepad.dpadDown;
vertslidesLeftStick = gamepad.leftStick;
}

Expand Down
10 changes: 8 additions & 2 deletions Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/DualTeleOp.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
import org.firstinspires.ftc.sixteen750.controls.DriverController;
import org.firstinspires.ftc.sixteen750.controls.OperatorController;
import org.firstinspires.ftc.sixteen750.helpers.StartingPosition;
import org.firstinspires.ftc.sixteen750.subsystems.HorizontalSlidesSubsystem;

@TeleOp(name = "Dual Control")
@SuppressWarnings("unused")
Expand Down Expand Up @@ -48,7 +49,12 @@ public void uponInit() {

@Override
public void uponStart() {
robot.horizontalSlidesSubsystem.slidesin();
robot.horizontalSlidesSubsystem.ClawWristServoTransfer();
if (Setup.Connected.HORIZONTALSLIDESUBSYSTEM == true) {
robot.horizontalSlidesSubsystem.slidesin();
robot.horizontalSlidesSubsystem.ClawWristServoTransfer();
}
if (Setup.Connected.VERTICALSLIDESUBSYSTEM){
robot.verticalSlidesSubsystem.slidesDown();
}
}
}
3 changes: 2 additions & 1 deletion Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/VertTeleOp.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,8 +51,9 @@ public void uponInit() {

@Override
public void uponStart() {
if (Setup.Connected.HORIZONTALSLIDESUBSYSTEM){
robot.horizontalSlidesSubsystem.slidesin();
robot.horizontalSlidesSubsystem.ClawWristServoTransfer();
robot.horizontalSlidesSubsystem.ClawWristServoTransfer();}
robot.verticalSlidesSubsystem.slidesDown();
}
}
26 changes: 17 additions & 9 deletions ...50/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/VerticalSlidesSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,10 +19,10 @@ public class VerticalSlidesSubsystem implements Subsystem, Loggable {
//arm servo - transfer, pickup, neutral
//bucket servo - drop, pickup (long and short)

public static double LOW_BASKET = -950;
public static double HIGH_BASKET = -1350;
public static int LOW_BASKET = -450;
public static int HIGH_BASKET = -850;
// public static double HIGH_POS = 1000;
public static double SLIDE_ZERO = 0;
public static int SLIDE_ZERO = 0;
public static double SLIDE_POS = 0;
public static double ARM_POS = 0;
public static double MIN_SERVO_SPEED = -.5;
Expand Down Expand Up @@ -56,13 +56,13 @@ public class VerticalSlidesSubsystem implements Subsystem, Loggable {
@Log(name = "bucketTarget")
public double bucketTargetPos;

public static PIDCoefficients PID = new PIDCoefficients(0.0, 0.0, 0.0);
public Servo armServo;
public Servo bucketServo;
public EncodedMotor<DcMotorEx> slideMotor;
private boolean isHardware;
public static PIDCoefficients slidePID = new PIDCoefficients(0.0, 0.0, 0.0);
public static PIDCoefficients slidePID = new PIDCoefficients(0.0001, 0.0, 0.0);
private PIDFController slidePidController;
public static double FEEDFORWARD_COEFFICIENT = 0.13;
public int slideResetPos;

public VerticalSlidesSubsystem(Hardware hw) {
Expand All @@ -73,7 +73,9 @@ public VerticalSlidesSubsystem(Hardware hw) {
armServo = hw.armservo;
bucketServo = hw.bucketservo;
isHardware = true;
slidePidController = new PIDFController(PID, 0, 0, 0, (x, y) -> 0.1);
slidePidController = new PIDFController(slidePID, 0, 0, 0, (ticks, velocity) ->
FEEDFORWARD_COEFFICIENT
);
resetSlideZero();
}

Expand Down Expand Up @@ -124,16 +126,22 @@ private void setSlideTargetPosition(int p) {
public void slidesDown() {
// lowers the bucket system
//probably going to do the slide thing with the joysticks (negative of slidesup)
setSlideMotorPower(SLIDE_ZERO);
setSlidePos(SLIDE_ZERO);
}

public void slideBasketLow() {
//takes the arm to the first level
setSlideMotorPower(LOW_BASKET);
setSlidePos(LOW_BASKET);
}

public void slidesUp() {
// lowers the bucket system
//probably going to do the slide thing with the joysticks (negative of slidesup)
setSlidePos(LOW_BASKET);
}

public void slideBasketHigh() {
setSlideMotorPower(HIGH_BASKET);
setSlidePos(HIGH_BASKET);
}

public void slideChamberLow() {
Expand Down

0 comments on commit 1132143

Please sign in to comment.