diff --git a/src/main/java/com/team766/robot/filip/LightsCANdle.java b/src/main/java/com/team766/robot/filip/LightsCANdle.java new file mode 100644 index 00000000..cd9dcacc --- /dev/null +++ b/src/main/java/com/team766/robot/filip/LightsCANdle.java @@ -0,0 +1,43 @@ +package com.team766.robot.filip; +import java.util.Set; +import static com.team766.framework.RulePersistence.*; + +import com.team766.framework.RuleGroup; +import com.team766.hal.JoystickReader; +import com.team766.hal.RobotProvider; +import com.team766.robot.common.mechanisms.LEDString; +import com.team766.robot.common.constants.ColorConstants; +import com.ctre.phoenix.led.Animation; +import com.ctre.phoenix.led.RainbowAnimation; +import com.team766.robot.common.constants.InputConstants; + +public class LightsCANdle extends RuleGroup{ + private final LEDString lights = new LEDString("lights"); + private final LEDString.Segment lightStrip = lights.makeSegment(0,7); + + public LightsCANdle () { + final Animation rainbow = new RainbowAnimation(); + final JoystickReader joystick = RobotProvider.instance.getJoystick(0); + + addRule("Yellow for Cone", + joystick.whenButton(InputConstants.GAMEPAD_A_BUTTON), + ONCE, + Set.of(lights), + () -> {lightStrip.setColor(ColorConstants.YELLOW);} + ); + + addRule("Purple for Cube", + joystick.whenButton(InputConstants.GAMEPAD_B_BUTTON), + ONCE, + Set.of(lights), + () -> {lightStrip.setColor(ColorConstants.PURPLE);} + ); + + addRule("Rainbow for Defense", + joystick.whenButton(InputConstants.GAMEPAD_X_BUTTON), + ONCE, + Set.of(lights), + () -> {lightStrip.animate(rainbow);} + ); + } +} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/filip/OI.java b/src/main/java/com/team766/robot/filip/OI.java new file mode 100644 index 00000000..9ea138aa --- /dev/null +++ b/src/main/java/com/team766/robot/filip/OI.java @@ -0,0 +1,29 @@ +package com.team766.robot.filip; + +import static com.team766.framework.RulePersistence.*; + +import com.team766.framework.RuleGroup; +import com.team766.hal.JoystickReader; +import com.team766.hal.RobotProvider; +import com.team766.robot.Kevan.mechanisms.Drive; +import com.team766.robot.common.constants.InputConstants; +import java.util.Set; + +public class OI_A extends RuleGroup { + public OI_A(Drive drive) { + final JoystickReader joystick1 = RobotProvider.instance.getJoystick(0); + addRule( + "handle_axis_moved", + joystick1.whenAnyAxisMoved( + InputConstants.AXIS_FORWARD_BACKWARD, InputConstants.AXIS_LEFT_RIGHT), + ONCE_AND_HOLD, + Set.of(drive), + () -> { + double forward_backward = + joystick1.getAxis(InputConstants.AXIS_FORWARD_BACKWARD); + double left_right = joystick1.getAxis(InputConstants.AXIS_LEFT_RIGHT); + drive.move_left(forward_backward + left_right); + drive.move_right(forward_backward - left_right); + }); + } +} diff --git a/src/main/java/com/team766/robot/filip/OI_MAyhem b/src/main/java/com/team766/robot/filip/OI_MAyhem new file mode 100644 index 00000000..82224c96 --- /dev/null +++ b/src/main/java/com/team766/robot/filip/OI_MAyhem @@ -0,0 +1,66 @@ +package com.team766.robot.filip; + +import static com.team766.framework.RulePersistence.*; + +import com.team766.framework.Context; +import com.team766.framework.RuleGroup; +import com.team766.hal.JoystickReader; +import com.team766.hal.RobotProvider; +import com.team766.robot.Kevan.mechanisms.Drive; +import com.team766.robot.Kevan.mechanisms.Intake; +import com.team766.robot.Kevan.mechanisms.Shooter; +import com.team766.robot.common.constants.InputConstants; +import java.util.Set; + +// Tank Drive OI +public class OI_MAyhem extends RuleGroup { + public OI_MAYHEM(Drive drive, Shooter shooter, Intake intake, Context context) { + final JoystickReader gamePad1 = RobotProvider.instance.getJoystick(0); + addRule( + "RUN_LEFT_MOTOR", + gamePad1.whenAxisMoved(InputConstants.GAMEPAD_LEFT_STICK_YAXIS), + ONCE_AND_HOLD, + Set.of(drive), + () -> { + drive.move_right(gamePad1.getAxis(InputConstants.GAMEPAD_LEFT_STICK_YAXIS)); + }); + + addRule( + "RUN_RIGHT_MOTOR", + gamePad1.whenAxisMoved(InputConstants.GAMEPAD_RIGHT_STICK_YAXIS), + ONCE_AND_HOLD, + Set.of(drive), + () -> { + drive.move_left(gamePad1.getAxis(InputConstants.GAMEPAD_RIGHT_STICK_YAXIS)); + }); + addRule( + "RUN_INTAKE", + gamePad1.whenButton(InputConstants.GAMEPAD_RIGHT_BUMPER_BUTTON), + ONCE_AND_HOLD, + Set.of(intake), + () -> { + intake.SetIntake(1); + }) + .withFinishedTriggeringProcedure( + intake, + () -> { + intake.SetIntake(0); + }); + addRule( + "SHOOT_SET_POWER", + gamePad1.whenAxisMoved(InputConstants.GAMEPAD_RIGHT_TRIGGER), + ONCE_AND_HOLD, + Set.of(shooter), + () -> { + shooter.SetShooterSpeed( + gamePad1.getAxis(InputConstants.GAMEPAD_RIGHT_TRIGGER)); + context.waitForSeconds(0.25); + shooter.SetTransferSpeed(1); + }) + .withFinishedTriggeringProcedure( + shooter, + () -> { + shooter.SetTransferSpeed(0); + }); + } +} diff --git a/src/main/java/com/team766/robot/filip/Shooter.java b/src/main/java/com/team766/robot/filip/Shooter.java new file mode 100644 index 00000000..85b74876 --- /dev/null +++ b/src/main/java/com/team766/robot/filip/Shooter.java @@ -0,0 +1,28 @@ +package com.team766.robot.filip.mechanisms; + +import com.team766.framework.MechanismWithStatus; +import com.team766.framework.Status; +import com.team766.hal.MotorController; +import com.team766.hal.RobotProvider; + +public class Shooter extends MechanismWithStatus { + + MotorController shooter_motor = RobotProvider.instance.getMotor("shooter_motor"); + MotorController tranfer_motor = RobotProvider.instance.getMotor("transfer_motor"); + + public record ShooterStatus(double currentPosition) implements Status {} + + public Shooter() {} + + public void SetShooterSpeed(double motorPower) { + shooter_motor.set(motorPower); + } + + public void SetTransferSpeed(double motorPower) { + tranfer_motor.set(motorPower); + } + + protected ShooterStatus updateStatus() { + return new ShooterStatus(0); + } +} diff --git a/src/main/java/com/team766/robot/filip/mechanisms/Drive.java b/src/main/java/com/team766/robot/filip/mechanisms/Drive.java new file mode 100644 index 00000000..13653b59 --- /dev/null +++ b/src/main/java/com/team766/robot/filip/mechanisms/Drive.java @@ -0,0 +1,27 @@ + +package com.team766.robot.filip.mechanisms; + +import com.team766.framework.MechanismWithStatus; +import com.team766.framework.Status; +import com.team766.hal.MotorController; +import com.team766.hal.RobotProvider; + +public class Drive extends MechanismWithStatus { + MotorController motor_left = RobotProvider.instance.getMotor("leftMotor"); + MotorController motor_right = RobotProvider.instance.getMotor("rightMotor"); + public record DriveStatus(double pos_motor_left, double pos_motor_right) implements Status {} + + public Drive() {} + + public void move_left(double speed) { + motor_left.set(speed); + } + + public void move_right(double speed) { + motor_right.set(speed); + } + + protected DriveStatus updateStatus() { + return new DriveStatus(motor_left.getSensorPosition(), motor_right.getSensorPosition()); + } +} diff --git a/src/main/java/com/team766/robot/filip/mechanisms/FilipMovingMotor.java b/src/main/java/com/team766/robot/filip/mechanisms/FilipMovingMotor.java new file mode 100644 index 00000000..8ad3542d --- /dev/null +++ b/src/main/java/com/team766/robot/filip/mechanisms/FilipMovingMotor.java @@ -0,0 +1,27 @@ +package com.team766.robot.filip.mechanisms; + +import com.team766.framework.MechanismWithStatus; +import com.team766.framework.Status; +import com.team766.hal.MotorController; +import com.team766.hal.RobotProvider; + +public class FilipMovingMotor extends MechanismWithStatus { + + public FilipMovingMotor() {} + + MotorController motor = RobotProvider.instance.getMotor("mainMotor"); + + public void move(double motorPower) { + motor.set(motorPower); + } + + public record MovingMotorStatus(double currentPosition) implements Status {} + + public void stop() { + motor.set(0); + } + + protected MovingMotorStatus updateStatus() { + return new MovingMotorStatus(motor.getSensorPosition()); + } +} diff --git a/src/main/java/com/team766/robot/filip/mechanisms/Intake.java b/src/main/java/com/team766/robot/filip/mechanisms/Intake.java new file mode 100644 index 00000000..aa37285a --- /dev/null +++ b/src/main/java/com/team766/robot/filip/mechanisms/Intake.java @@ -0,0 +1,23 @@ +package com.team766.robot.Kevan.mechanisms; + +import com.team766.framework.MechanismWithStatus; +import com.team766.framework.Status; +import com.team766.hal.MotorController; +import com.team766.hal.RobotProvider; + +public class Intake extends MechanismWithStatus { + + MotorController intake_motor = RobotProvider.instance.getMotor("intake_motor"); + + public record IntakeStatus(double currentPosition) implements Status {} + + public Intake() {} + + public void SetIntake(double motorPower) { + intake_motor.set(motorPower); + } + + protected IntakeStatus updateStatus() { + return new IntakeStatus(0); + } +} diff --git a/src/main/java/com/team766/robot/filip/procedures/Autonomous.java b/src/main/java/com/team766/robot/filip/procedures/Autonomous.java new file mode 100644 index 00000000..9d9ce2ff --- /dev/null +++ b/src/main/java/com/team766/robot/filip/procedures/Autonomous.java @@ -0,0 +1,35 @@ +package com.team766.robot.filip.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.Kevan.mechanisms.Drive; +import com.team766.robot.Kevan.mechanisms.Intake; +import com.team766.robot.Kevan.mechanisms.Shooter; + +public class Autonomous extends Procedure { + private Drive drive; + private Shooter shooter; + private Intake intake; + + public Autonomous(Drive myDrive, Shooter myShooter, Intake myIntake) { + this.drive = reserve(myDrive); + this.shooter = reserve(myShooter); + this.intake = reserve(myIntake); + } + + public void run(Context context) { + context.waitForSeconds(1); + context.runParallel(new DriveProcedure(drive, 1.25)); + context.runParallel(new ShootProcedure(shooter, 0.8)); + context.runParallel(new TurnProcedure(drive, 0.25, 0.5)); + context.runParallel(new DriveProcedure(drive, 0.3125)); + context.runParallel(new IntakeProcedure(intake)); + context.runParallel(new DriveProcedure(drive, 0.3125)); + context.runParallel(new IntakeProcedure(intake)); + context.runParallel(new TurnProcedure(drive, 0.5, 0.5)); + context.runParallel(new DriveProcedure(drive, 0.625)); + context.runParallel(new TurnProcedure(drive, 0.25, 0.5)); + context.runParallel(new ShootProcedure(shooter, 0.8)); + context.runParallel(new ShootProcedure(shooter, 0.8)); + } +} diff --git a/src/main/java/com/team766/robot/filip/procedures/DriveProcedure.java b/src/main/java/com/team766/robot/filip/procedures/DriveProcedure.java new file mode 100644 index 00000000..b25bf440 --- /dev/null +++ b/src/main/java/com/team766/robot/filip/procedures/DriveProcedure.java @@ -0,0 +1,22 @@ +package com.team766.robot.Kevan.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.Kevan.mechanisms.Drive; + +public class DriveProcedure extends Procedure { + + private Drive drive; + private double seconds; + + public DriveProcedure(Drive myDrive, double seconds) { + drive = reserve(myDrive); + this.seconds = seconds; + } + + public void run(Context context) { + drive.move_straight(0.8); + context.waitForSeconds(seconds); + drive.move_straight(0); + } +} diff --git a/src/main/java/com/team766/robot/filip/procedures/IntakeProcedure.java b/src/main/java/com/team766/robot/filip/procedures/IntakeProcedure.java new file mode 100644 index 00000000..33fc09e2 --- /dev/null +++ b/src/main/java/com/team766/robot/filip/procedures/IntakeProcedure.java @@ -0,0 +1,20 @@ +package com.team766.robot.Kevan.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.Kevan.mechanisms.Intake; + +public class IntakeProcedure extends Procedure { + + private Intake intake; + + public IntakeProcedure(Intake myIntake) { + intake = reserve(myIntake); + } + + public void run(Context context) { + intake.SetIntake(1); + context.waitForSeconds(0.5); + intake.SetIntake(0); + } +} diff --git a/src/main/java/com/team766/robot/filip/procedures/MoveMotor.java b/src/main/java/com/team766/robot/filip/procedures/MoveMotor.java new file mode 100644 index 00000000..eb1b2b9f --- /dev/null +++ b/src/main/java/com/team766/robot/filip/procedures/MoveMotor.java @@ -0,0 +1,17 @@ +package com.team766.robot.filip.procedures; +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.filip.mechanisms.FilipMovingMotor; + +public class MoveMotor extends Procedure { + private FilipMovingMotor motor; + public MoveMotor(FilipMovingMotor myMotor){ + motor = reserve(myMotor); + } + @Override + public void run(Context context) { + motor.moveSpeed(1); + context.waitForSeconds(5); + motor.moveSpeed(0); + } +} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/filip/procedures/ShootProcedure.java b/src/main/java/com/team766/robot/filip/procedures/ShootProcedure.java new file mode 100644 index 00000000..75c99d2c --- /dev/null +++ b/src/main/java/com/team766/robot/filip/procedures/ShootProcedure.java @@ -0,0 +1,25 @@ +package com.team766.robot.filip.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.Kevan.mechanisms.Shooter; + +public class ShootProcedure extends Procedure { + + private Shooter shooter; + private double power; + + public ShootProcedure(Shooter myShooter, double power) { + shooter = reserve(myShooter); + this.power = power; + } + + public void run(Context context) { + shooter.SetShooterSpeed(power); + context.waitForSeconds(0.25); + shooter.SetTransferSpeed(1); + context.waitForSeconds(0.5); + shooter.SetTransferSpeed(0); + shooter.SetShooterSpeed(0); + } +} diff --git a/src/main/java/com/team766/robot/filip/procedures/TurnProcedure.java b/src/main/java/com/team766/robot/filip/procedures/TurnProcedure.java new file mode 100644 index 00000000..20f29ebf --- /dev/null +++ b/src/main/java/com/team766/robot/filip/procedures/TurnProcedure.java @@ -0,0 +1,24 @@ +package com.team766.robot.filip.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.Kevan.mechanisms.Drive; + +public class TurnProcedure extends Procedure { + + private Drive drive; + private double seconds; + private double motorPower; + + public TurnProcedure(Drive myDrive, double seconds, double motorPower) { + drive = reserve(myDrive); + this.seconds = seconds; + this.motorPower = motorPower; + } + + public void run(Context context) { + drive.turn_right(motorPower); + context.waitForSeconds(seconds); + drive.move_straight(0); + } +}