Skip to content

Commit

Permalink
Braelyns stuff rebased (#82)
Browse files Browse the repository at this point in the history 
* 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

* Untested shifting of modes

* 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

* no clue what I changed but I'm committing so I can checkout main

* 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

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

* things happened, merge funnies, choice commands were started but not tested. lots of things to do, not enough time...

* things work!!! still have a lot to do but feedfwd down is working, so is feedfwd up, bucket positions are correct and tested. added new sequential thingies, fixed slide pid, moved some buttons around, need to change some things for dual tele, vertical tele now has inc/dec for bucket. values were tested for vertical things but we still need to test scoring with high basket. changed some button binding for OperatorController and we might have some issues with horizontal positions right now that we'll need to fix

* added some sequentials and stuff, i think the problem with the choice commands is we have two separate button binding things that are private, so they can't access each other to communicate. need to connect a couple sequentials and check the high basket pos once we have functional slides again

---------

Co-authored-by: Braelyn <[email protected]>
  • Loading branch information
@kevinfrei @braelynandthefrogs
kevinfrei and braelynandthefrogs authored Nov 23, 2024
1 parent b99d120 commit 1a142eb
Show file tree
Hide file tree
Showing 8 changed files with 133 additions and 49 deletions.
3 changes: 3 additions & 0 deletions .../main/java/org/firstinspires/ftc/sixteen750/commands/slides/HorizontalSlidesCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,9 @@ public static Command wristTransfer(Robot r) {
r.horizontalSlidesSubsystem
);
}
public static Command VertExtendTransfer(Robot r) {
return Command.create(r.horizontalSlidesSubsystem::VertExtendTransfer, r.horizontalSlidesSubsystem);
}

public static SequentialCommandGroup transferring(Robot r) {
return new SequentialCommandGroup(
Expand Down
16 changes: 13 additions & 3 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 @@ -37,6 +37,18 @@ public static Command BucketTransfer(Robot r) {
r.verticalSlidesSubsystem
);
}
public static Command BucketDecrement(Robot r) {
return Command.create(
r.verticalSlidesSubsystem::bucketServoDecrement,
r.verticalSlidesSubsystem
);
}
public static Command BucketIncrement(Robot r) {
return Command.create(
r.verticalSlidesSubsystem::bucketServoIncrement,
r.verticalSlidesSubsystem
);
}

public static Command BucketEmpty(Robot r) {
return Command.create(
Expand All @@ -55,16 +67,14 @@ public static Command ArmTransfer(Robot r) {
public static Command ArmScore(Robot r) {
return Command.create(r.verticalSlidesSubsystem::armServoEmpty, r.verticalSlidesSubsystem);
}
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
35 changes: 27 additions & 8 deletions ...main/java/org/firstinspires/ftc/sixteen750/commands/slides/VerticalSlidesSequentials.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,33 +2,52 @@

import com.technototes.library.command.Command;
import com.technototes.library.command.SequentialCommandGroup;
import com.technototes.library.command.WaitCommand;

import org.firstinspires.ftc.sixteen750.Robot;

public class VerticalSlidesSequentials {

public static SequentialCommandGroup transferVertical(Robot r) {
return new SequentialCommandGroup(
Command.create(r.verticalSlidesSubsystem::bucketServoTransfer, r.verticalSlidesSubsystem),
Command.create(r.verticalSlidesSubsystem::armServoTransfer, r.verticalSlidesSubsystem),
Command.create(r.verticalSlidesSubsystem::slidesDown, r.verticalSlidesSubsystem)
Command.create(r.horizontalSlidesSubsystem::VertExtendTransfer, r.horizontalSlidesSubsystem),
Command.create(r.verticalSlidesSubsystem::armServoTransfer, r.verticalSlidesSubsystem),
new WaitCommand(.3),
Command.create(r.verticalSlidesSubsystem::slidesDown, r.verticalSlidesSubsystem),
new WaitCommand(.3),
Command.create(r.verticalSlidesSubsystem::bucketServoTransfer, r.verticalSlidesSubsystem)
// commands for vertical slide bucket transfer position first, then wrist transferring
);
}
public static SequentialCommandGroup LowBasket(Robot r) {
return new SequentialCommandGroup(
Command.create(r.verticalSlidesSubsystem::bucketServoTransfer, r.verticalSlidesSubsystem),
Command.create(r.verticalSlidesSubsystem::armServoEmpty, r.verticalSlidesSubsystem),
BasketScore(r),
Command.create(r.verticalSlidesSubsystem::slideBasketLow, r.verticalSlidesSubsystem)
);
}
public static SequentialCommandGroup HighBasket(Robot r) {

public static SequentialCommandGroup HighBasket(Robot r) { //need to change armScore
return new SequentialCommandGroup(
Command.create(r.verticalSlidesSubsystem::bucketServoTransfer, r.verticalSlidesSubsystem),
Command.create(r.verticalSlidesSubsystem::armServoEmpty, r.verticalSlidesSubsystem),
BasketScore(r),
Command.create(r.verticalSlidesSubsystem::slideBasketHigh, r.verticalSlidesSubsystem)
);
}
public static SequentialCommandGroup HighDown(Robot r) {
return new SequentialCommandGroup(
Command.create(r.verticalSlidesSubsystem::slideBasketLow, r.verticalSlidesSubsystem),
new WaitCommand(.1),
Command.create(r.verticalSlidesSubsystem::slidesDown, r.verticalSlidesSubsystem)
);
}
public static SequentialCommandGroup BasketScore(Robot r) {
return new SequentialCommandGroup(
Command.create(r.verticalSlidesSubsystem::bucketServoLift, r.verticalSlidesSubsystem),
new WaitCommand(0.3),
Command.create(r.verticalSlidesSubsystem::armServoEmpty, r.verticalSlidesSubsystem),
new WaitCommand(0.5),
Command.create(r.verticalSlidesSubsystem::bucketServoEmpty, r.verticalSlidesSubsystem)
);
}

//transfer
//inc/dec
Expand Down
47 changes: 36 additions & 11 deletions Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/OperatorController.java
View file
Original file line number Diff line number Diff line change
@@ -1,9 +1,13 @@
package org.firstinspires.ftc.sixteen750.controls;

import android.util.Pair;
import com.technototes.library.command.ChoiceCommand;
import com.technototes.library.command.CommandScheduler;
import com.technototes.library.control.CommandButton;
import com.technototes.library.control.CommandGamepad;
import com.technototes.library.control.Stick;
import com.technototes.library.logger.Log;

import org.firstinspires.ftc.sixteen750.Robot;
import org.firstinspires.ftc.sixteen750.Setup;
import org.firstinspires.ftc.sixteen750.commands.driving.JoystickDriveCommand;
Expand All @@ -15,8 +19,12 @@ public class OperatorController {

public Robot robot;
public CommandGamepad gamepad;

public CommandButton shiftButton;

public CommandButton openClaw_closeClaw;


//horizontal buttons
public Stick horislidesLeftStick;
public CommandButton openClaw;
Expand Down Expand Up @@ -46,6 +54,9 @@ public class OperatorController {

//choice commands
public CommandButton horizontalSlides_verticalSlides;

public CommandButton openClaw_bucketLow;

public boolean shifted = false;

public boolean isShifted() {
Expand All @@ -66,22 +77,30 @@ public OperatorController(CommandGamepad g, Robot r) {
}

private void AssignNamedControllerButton() {
openClaw = gamepad.leftBumper;
openClaw_bucketLow = gamepad.leftBumper;
closeClaw = gamepad.rightBumper;
wristTransfer = gamepad.ps_triangle;
wristTransfer = gamepad.dpadLeft;
wristPickup = gamepad.ps_circle;
wristIncrement = gamepad.dpadRight;
wristDecrement = gamepad.dpadLeft;
horislidesLeftStick = gamepad.leftStick;
horislidesExtend = gamepad.ps_cross;
horislidesRetract = gamepad.ps_square;

//vertical commands
bucketTransfer = gamepad.ps_circle;
bucketScore = gamepad.ps_triangle;


//bucketIncrement = gamepad.dpadDown;
//bucketDecrement = gamepad.dpadDown;

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

Expand All @@ -99,20 +118,26 @@ private void BindButtons() {
}
//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));
shiftButton.whenPressed(this::toggleShift);
horizontalSlides_verticalSlides.whenPressed(
new ChoiceCommand(
new Pair<>(this::notShifted, HorizontalSlidesCommands.intake(robot)),
new Pair<>(this::isShifted, VerticalSlidesCommands.HighBasket(robot))
)
);
closeClaw.whenPressed(HorizontalSlidesCommands.clawChomp(robot));
wristPickup.whenPressed(HorizontalSlidesCommands.wristPickup(robot));
wristTransfer.whenPressed(HorizontalSlidesCommands.wristTransfer(robot));
wristIncrement.whenPressed(HorizontalSlidesCommands.wristIncrement(robot));
wristDecrement.whenPressed(HorizontalSlidesCommands.wristDecrement(robot));
//wristIncrement.whenPressed(HorizontalSlidesCommands.wristIncrement(robot));
//wristDecrement.whenPressed(HorizontalSlidesCommands.wristDecrement(robot));
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));
/*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));
Expand Down
26 changes: 19 additions & 7 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 @@ -7,6 +7,7 @@
import org.firstinspires.ftc.sixteen750.Robot;
import org.firstinspires.ftc.sixteen750.Setup;
import org.firstinspires.ftc.sixteen750.commands.slides.VerticalSlidesCommands;
import org.firstinspires.ftc.sixteen750.commands.slides.VerticalSlidesSequentials;

public class OperatorControllerVertical {

Expand All @@ -22,6 +23,8 @@ public class OperatorControllerVertical {
public CommandButton slidesLow;
public CommandButton slidesHigh;
public CommandButton slidesDown;
public CommandButton transferVertical;
public CommandButton basketScore;
//public CommandButton wristDecrement;

public OperatorControllerVertical(CommandGamepad g, Robot r) {
Expand All @@ -34,14 +37,16 @@ public OperatorControllerVertical(CommandGamepad g, Robot r) {
private void AssignNamedControllerButton() {
bucketTransfer = gamepad.ps_circle;
bucketScore = gamepad.ps_triangle;
armTransfer = gamepad.ps_cross;
armScore = gamepad.ps_square;
//armTransfer = gamepad.ps_cross;
//armScore = gamepad.ps_square;
slidesHigh = gamepad.ps_share;
slidesLow = gamepad.dpadUp;
slidesDown = gamepad.dpadDown;
bucketIncrement = gamepad.dpadDown;
bucketDecrement = gamepad.dpadDown;
bucketIncrement = gamepad.rightBumper; //actually functions as decrement
bucketDecrement = gamepad.leftBumper; //actually functions as increment
vertslidesLeftStick = gamepad.leftStick;
transferVertical = gamepad.ps_cross;
basketScore = gamepad.ps_square;
}

private void BindButtons() {
Expand All @@ -53,13 +58,20 @@ private void BindButtons() {


private void bindVerticalSlidesControls() {
transferVertical.whenPressed(VerticalSlidesSequentials.transferVertical(robot));
bucketTransfer.whenPressed(VerticalSlidesCommands.BucketTransfer(robot));
bucketScore.whenPressed(VerticalSlidesCommands.BucketEmpty(robot));
armTransfer.whenPressed(VerticalSlidesCommands.ArmTransfer(robot));
armScore.whenPressed(VerticalSlidesCommands.ArmScore(robot));
//armTransfer.whenPressed(VerticalSlidesCommands.ArmTransfer(robot));
//armScore.whenPressed(VerticalSlidesCommands.ArmScore(robot));
basketScore.whenPressed(VerticalSlidesSequentials.BasketScore(robot));
slidesHigh.whenPressed(VerticalSlidesCommands.HighBasket(robot));
slidesLow.whenPressed(VerticalSlidesCommands.LowBasket(robot));
slidesDown.whenPressed(VerticalSlidesCommands.SlidesZero(robot));
slidesDown.whenPressed(VerticalSlidesSequentials.HighDown(robot));
//wristDecrement.whenPressed(SlidesCommands.wristDecrement(robot));
bucketIncrement.whenPressed(VerticalSlidesCommands.BucketIncrement(robot));
bucketDecrement.whenPressed(VerticalSlidesCommands.BucketDecrement(robot));
// slidesHigh.whenPressed(VerticalSlidesSequentials.HighBasket(robot));
// slidesLow.whenPressed(VerticalSlidesSequentials.LowBasket(robot));
// slidesDown.whenPressed(VerticalSlidesSequentials.transferVertical(robot));
}
}
3 changes: 2 additions & 1 deletion 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 @@ -49,7 +49,8 @@ public void uponInit() {

@Override
public void uponStart() {
if (Setup.Connected.HORIZONTALSLIDESUBSYSTEM == true) {
if (Setup.Connected.HORIZONTALSLIDESUBSYSTEM) {
robot.horizontalSlidesSubsystem.ClawServoChomp();
robot.horizontalSlidesSubsystem.slidesin();
robot.horizontalSlidesSubsystem.ClawWristServoTransfer();
}
Expand Down
11 changes: 8 additions & 3 deletions .../src/main/java/org/firstinspires/ftc/sixteen750/subsystems/HorizontalSlidesSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,10 +27,11 @@ public class HorizontalSlidesSubsystem implements Subsystem, Loggable {
public static double LinkServoExtend = 0.6;
public static double LinkServoRetract = 1;
public static double ClawServoClose = 0.3;
public static double ClawServoOpen = 0.8;
public static double ClawServoOpen = 0.6;
public static double WristServoTransfer = 0.3;
public static double WristServoPickup = 1;
public static double WristServoIncrement = 0.2;
public static double VertExtendTransfer = 0.1;
public static double WristServoPickup = .9;
public static double WristServoIncrement = 0.15;

@Log(name = "wristTarget")
public double wristTargetPos;
Expand Down Expand Up @@ -104,6 +105,10 @@ public void ClawWristServoTransfer() {
// positions for the arm of the bot for transfer
setWristPos(WristServoTransfer);
}
public void VertExtendTransfer() {
// positions for the arm of the bot for transfer
setWristPos(VertExtendTransfer);
}

public void ClawWristServoIncrement() {
// the arm's position to score
Expand Down
41 changes: 25 additions & 16 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 @@ -32,13 +32,13 @@ public class VerticalSlidesSubsystem implements Subsystem, Loggable {
// public static double ScoreServo = 0.5;
// public static double ArmServo = 0.5;
public static double ClawServoOpenShort = 0.4;
public static double BucketServoTransfer = 0.55;
public static double BucketServoEmpty = 0.05;
public static double BucketServoLift = 0;
public static double BucketServoTransfer = 0.85;
public static double BucketServoEmpty = 0.35;
public static double BucketServoLift = 0.65; //carry position for scoring
public static double ArmServoInput = 0.545;
public static double ArmServoEmpty = 1;
public static double BucketServoIncrement = 0.05;
public static double ArmServoIncrement = 0.555;
public static double ArmServoIncrement = 0.05;
public static double ArmServoTransfer = 0;

@Log(name = "slidePos")
Expand All @@ -60,9 +60,11 @@ public class VerticalSlidesSubsystem implements Subsystem, Loggable {
public Servo bucketServo;
public EncodedMotor<DcMotorEx> slideMotor;
private boolean isHardware;
public static PIDCoefficients slidePID = new PIDCoefficients(0.0001, 0.0, 0.0);
public static PIDCoefficients slidePID = new PIDCoefficients(0.0015, 0.0, 0.0);
private PIDFController slidePidController;
public static double FEEDFORWARD_COEFFICIENT = 0.13;
public static double FEEDFORWARD_COEFFICIENT = -0.13;
public static double FEEDFORWARD_DOWN = 0.07;
public static double FEEDFORWARD_UP = -0.13;
public int slideResetPos;

public VerticalSlidesSubsystem(Hardware hw) {
Expand All @@ -73,9 +75,16 @@ public VerticalSlidesSubsystem(Hardware hw) {
armServo = hw.armservo;
bucketServo = hw.bucketservo;
isHardware = true;
slidePidController = new PIDFController(slidePID, 0, 0, 0, (ticks, velocity) ->
FEEDFORWARD_COEFFICIENT
);
slidePidController =
new PIDFController(
slidePID,
0,
0,
0,

(ticks, velocity) ->
FEEDFORWARD_COEFFICIENT
);
resetSlideZero();
}

Expand All @@ -100,6 +109,12 @@ public void periodic() {
}

private void setSlidePos(int e) {
if (getSlideCurrentPos() < e){
FEEDFORWARD_COEFFICIENT = FEEDFORWARD_DOWN;
}
else {
FEEDFORWARD_COEFFICIENT = FEEDFORWARD_UP;
}
slidePidController.setTargetPosition(e);
slideTargetPos = e;
}
Expand Down Expand Up @@ -134,19 +149,13 @@ public void slideBasketLow() {
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() {
setSlidePos(HIGH_BASKET);
}

public void slideChamberLow() {
//takes the arm to the first level
slidePidController.setTargetPosition(LOW_BASKET);
setSlidePos(LOW_BASKET);
}

public void slideChamberHigh() {
Expand Down

0 comments on commit 1a142eb

Please sign in to comment.