Skip to content

Harshini and Viggo's branch, added commands - wrist now works #113

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
Magkhuu wants to merge 2 commits into from
Closed

Harshini and Viggo's branch, added commands - wrist now works #113

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
8205e64
wrist works :DDD
TechnototesLaptop Dec 11, 2024
e3c38f0
viggo
TechnototesLaptop Dec 11, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
30 changes: 30 additions & 0 deletions Twenty403/src/main/java/org/firstinspires/ftc/twenty403/commands/ArmSubCmds.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,30 @@
package org.firstinspires.ftc.twenty403.commands;

import com.technototes.library.command.Command;

import org.firstinspires.ftc.twenty403.subsystems.ArmSubsystem;

public class ArmSubCmds {

public static class cmds {

public static Command slideZero(ArmSubsystem AS) {
return Command.create(AS::setSlideToZero);
}
public static Command armIntake(ArmSubsystem AS) {
return Command.create(AS::setArmToIntake);
}
public static Command highbasketSlide(ArmSubsystem AS) {
return Command.create(AS::highBasketSlides);
}
public static Command highbasketArm(ArmSubsystem AS) {
return Command.create(AS::highBasket);
}
public static Command lowbasketArm(ArmSubsystem AS) {
return Command.create(AS::lowBasket);
}



}
}
17 changes: 9 additions & 8 deletions Twenty403/src/main/java/org/firstinspires/ftc/twenty403/commands/HighBasketCommand.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,20 +10,21 @@ public class HighBasketCommand {

public static SequentialCommandGroup HighBasketPreArm(Robot r) {
return new SequentialCommandGroup(
Command.create(r.kidShampooSubsystem::closeRetainer, r.kidShampooSubsystem),
Command.create(r.kidShampooSubsystem::spitIntake, r.kidShampooSubsystem),
Command.create(r.kidShampooSubsystem::dumpWrist, r.kidShampooSubsystem)
KidShampooCmds.cmds.DumpWrist(r.kidShampooSubsystem),
KidShampooCmds.cmds.OpenRetainer(r.kidShampooSubsystem)
);
}

public static SequentialCommandGroup HighBasket(Robot r) {
return new SequentialCommandGroup(
Command.create(r.kidShampooSubsystem::stopIntake, r.kidShampooSubsystem),
new MoveSlidesCommand(r.armSubsystem, r.armSubsystem::setSlideToZero),
Command.create(r.armSubsystem::highBasket, r.armSubsystem),
new WaitCommand(0.5),
new MoveSlidesCommand(r.armSubsystem, r.armSubsystem::highBasketSlides),
KidShampooCmds.cmds.CloseRetainer(r.kidShampooSubsystem),
KidShampooCmds.cmds.StopIntake(r.kidShampooSubsystem),
KidShampooCmds.cmds.ScoopWrist(r.kidShampooSubsystem),
ArmSubCmds.cmds.slideZero(r.armSubsystem),
ArmSubCmds.cmds.highbasketArm(r.armSubsystem),
new WaitCommand(0.5),
ArmSubCmds.cmds.highbasketSlide(r.armSubsystem),
new WaitCommand(1.2),
HighBasketPreArm(r)
);
}
Expand Down
6 changes: 6 additions & 0 deletions Twenty403/src/main/java/org/firstinspires/ftc/twenty403/commands/KidShampooCmds.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,5 +37,11 @@ public static Command SpitIntake(KidShampooSubsystem KS) {
public static Command StopIntake(KidShampooSubsystem KS) {
return Command.create(KS::stopIntake);
}
public static Command DumpWrist(KidShampooSubsystem KS) {
return Command.create(KS::dumpWrist);
}
public static Command ScoopWrist(KidShampooSubsystem KS) {
return Command.create(KS::scoopWrist);
}
}
}
17 changes: 8 additions & 9 deletions Twenty403/src/main/java/org/firstinspires/ftc/twenty403/commands/LowBasketCommand.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,24 +6,23 @@
import com.technototes.library.command.WaitCommand;
import org.firstinspires.ftc.twenty403.Robot;


public class LowBasketCommand {

public static SequentialCommandGroup LowBasketPreArm(Robot r) {
return new SequentialCommandGroup(
Command.create(r.kidShampooSubsystem::closeRetainer, r.kidShampooSubsystem),
Command.create(r.kidShampooSubsystem::spitIntake, r.kidShampooSubsystem),
Command.create(r.kidShampooSubsystem::dumpWrist, r.kidShampooSubsystem)
KidShampooCmds.cmds.DumpWrist(r.kidShampooSubsystem),
KidShampooCmds.cmds.OpenRetainer(r.kidShampooSubsystem)
);
}

public static SequentialCommandGroup LowBasket(Robot r) {
return new SequentialCommandGroup(
Command.create(r.kidShampooSubsystem::stopIntake),
new MoveSlidesCommand(r.armSubsystem, r.armSubsystem::setSlideToZero),
new WaitCommand(0.5),
Command.create(r.armSubsystem::lowBasket, r.armSubsystem),
new WaitCommand(0.5),
new MoveSlidesCommand(r.armSubsystem, r.armSubsystem::lowBasketSlides),
KidShampooCmds.cmds.CloseRetainer(r.kidShampooSubsystem),
KidShampooCmds.cmds.StopIntake(r.kidShampooSubsystem),
KidShampooCmds.cmds.ScoopWrist(r.kidShampooSubsystem),
ArmSubCmds.cmds.slideZero(r.armSubsystem),
ArmSubCmds.cmds.lowbasketArm(r.armSubsystem),
new WaitCommand(0.5),
LowBasketPreArm(r)
);
Expand Down
64 changes: 34 additions & 30 deletions Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/OperatorController.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
import org.firstinspires.ftc.twenty403.commands.JoystickIncDecCommand;
import org.firstinspires.ftc.twenty403.commands.JoystickSlideIncDecCommand;
import org.firstinspires.ftc.twenty403.commands.KidShampooCmds;
import org.firstinspires.ftc.twenty403.commands.ArmSubCmds;
import org.firstinspires.ftc.twenty403.commands.LowBasketCommand;
import org.firstinspires.ftc.twenty403.commands.LowSpecimenCommand;
import org.firstinspires.ftc.twenty403.commands.driving.JoystickDriveCommand;
Expand Down Expand Up @@ -83,14 +84,14 @@ private void AssignNamedControllerButton() {
armHorizontal = gamepad.dpadUp;
armStick = gamepad.rightStick;
slideStick = gamepad.leftStick;
/*HighBasket = gamepad.ps_circle;
armHighSpecimen = gamepad.ps_triangle;
armLowSpecimen = gamepad.ps_square;
armLowNet = gamepad.ps_cross;
IntakeSample = gamepad.ps_share;
HighBasket = gamepad.ps_circle;
// armHighSpecimen = gamepad.ps_triangle;
//armLowSpecimen = gamepad.ps_square;
armLowNet = gamepad.ps_square;
/*IntakeSample = gamepad.ps_share;
IntakeSpecimen = gamepad.ps_options;*/
WristDec = gamepad.ps_circle;
WristInc = gamepad.ps_square;
//WristDec = gamepad.ps_circle;
//WristInc = gamepad.ps_square;
}

public void BindControls() {
Expand All @@ -108,49 +109,57 @@ public void BindControls() {

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

spitIntake.whenReleased(
Command.create(robot.kidShampooSubsystem::stopIntake, robot.kidShampooSubsystem)
KidShampooCmds.cmds.StopIntake(robot.kidShampooSubsystem)
);
dumpWrist.whenPressed(
Command.create(robot.kidShampooSubsystem::dumpWrist, robot.kidShampooSubsystem)
//Command.create(robot.kidShampooSubsystem::dumpWrist, robot.kidShampooSubsystem)
KidShampooCmds.cmds.DumpWrist(robot.kidShampooSubsystem)
);
scoopWrist.whenPressed(
Command.create(robot.kidShampooSubsystem::scoopWrist, robot.kidShampooSubsystem)
//Command.create(robot.kidShampooSubsystem::scoopWrist, robot.kidShampooSubsystem)
KidShampooCmds.cmds.ScoopWrist(robot.kidShampooSubsystem)
);
/*straightWrist.whenPressed(
Command.create(robot.kidShampooSubsystem::straightWrist, robot.kidShampooSubsystem)
);*/
/*WristInc.whenPressed(
Command.create(robot.kidShampooSubsystem::wincrement, robot.kidShampooSubsystem)
);
WristDec.whenPressed(
Command.create(robot.kidShampooSubsystem::wdecrement, robot.kidShampooSubsystem)
);*/
}

public void bindArmControls() {
/*armLowNet.whenPressed(LowBasketCommand.LowBasket(robot));
armLowSpecimen.whenPressed(LowSpecimenCommand.LowSpecimen(robot));
armHighSpecimen.whenPressed(HighSpecimenCommand.HighSpecimen(robot));
armLowNet.whenPressed(LowBasketCommand.LowBasket(robot));
//armLowSpecimen.whenPressed(LowSpecimenCommand.LowSpecimen(robot));
// armHighSpecimen.whenPressed(HighSpecimenCommand.HighSpecimen(robot));
HighBasket.whenPressed(HighBasketCommand.HighBasket(robot));
IntakeSample.whenPressed(IntakeSampleCommand.IntakeSample(robot));
IntakeSpecimen.whenPressed(IntakeSpecimenCommand.IntakeSpecimen(robot));*/
//IntakeSample.whenPressed(IntakeSampleCommand.IntakeSample(robot));
// IntakeSpecimen.whenPressed(IntakeSpecimenCommand.IntakeSpecimen(robot));*/

armHorizontal.whenPressed(
Command.create(robot.armSubsystem::horizontal, robot.armSubsystem)
Expand All @@ -165,12 +174,7 @@ public void bindArmControls() {
CommandScheduler.scheduleJoystick(
new JoystickSlideIncDecCommand(robot.armSubsystem, slideStick)
);
WristInc.whenPressed(
Command.create(robot.kidShampooSubsystem::wincrement, robot.kidShampooSubsystem)
);
WristDec.whenPressed(
Command.create(robot.kidShampooSubsystem::wdecrement, robot.kidShampooSubsystem)
);

}

public void bindHangControls() {
Expand Down
4 changes: 2 additions & 2 deletions Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/ArmSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ public class ArmSubsystem implements Subsystem, Loggable {
private boolean isHardware;
public int slideResetPos;
public static double FEEDFORWARD_COEFFICIENT = 0.00014; //11-19-24
public static int ROTATE_MOTOR_LOW_BASKET_SCORING_POSITION = 3180;
public static int ROTATE_MOTOR_LOW_BASKET_SCORING_POSITION = 3100;
public static int ROTATE_MOTOR_HIGH_BASKET_SCORING_POSITION = 3100;
public static int ROTATE_MOTOR_SPECIMEN_SCORING_POSITION_LOW = 300;
public static int ROTATE_MOTOR_SPECIMEN_SCORING_POSITION_HIGH = 300;
Expand All @@ -30,7 +30,7 @@ public class ArmSubsystem implements Subsystem, Loggable {
public static double MAX_ARM_MOTOR_SPEED = 0.7;
public static int ROTATE_MOTOR_INTAKE_POSITION = 400;
public static int SLIDES_MOTOR_LOW_BASKET_SCORING_POSITION = 113;
public static int SLIDES_MOTOR_HIGH_BASKET_SCORING_POSITION = 1675;
public static int SLIDES_MOTOR_HIGH_BASKET_SCORING_POSITION = 1800;
public static int SLIDES_MOTOR_SPECIMEN_SCORING_POSITION_LOW = 2500;
public static int SLIDES_MOTOR_SPECIMEN_SCORING_POSITION_HIGH = 2500;
public static int SLIDES_MOTOR_INTAKE_POSITION = 600; //work on this
Expand Down
4 changes: 2 additions & 2 deletions Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/KidShampooSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,9 +43,9 @@ public class KidShampooSubsystem implements Subsystem, Loggable {

public static double INTAKE_SPIT = -.6;

public static double WRIST_SCOOP = 0;
public static double WRIST_SCOOP = 0.35;
public static double WRIST_INC_DEC = .5;
public static double WRIST_DUMP = -.7;
public static double WRIST_DUMP = 0.05;
public static double WRIST_STRAIGHT = .35;

@Log(name = "intakePos")
Expand Down
Loading