From da8bf67e5169f28a662ff719a145ea247f2e71d8 Mon Sep 17 00:00:00 2001 From: dejabot Date: Mon, 8 Jan 2024 20:08:40 -0800 Subject: [PATCH] temporarily stash changes for testing advantagekit --- src/main/java/com/team766/robot/OI.java | 32 +++++++++++++++---- src/main/java/com/team766/robot/Robot.java | 2 ++ .../robot/constants/InputConstants.java | 24 ++++++++++++++ .../com/team766/robot/mechanisms/Drive.java | 31 ++++++++++++++++++ 4 files changed, 82 insertions(+), 7 deletions(-) create mode 100644 src/main/java/com/team766/robot/constants/InputConstants.java create mode 100644 src/main/java/com/team766/robot/mechanisms/Drive.java diff --git a/src/main/java/com/team766/robot/OI.java b/src/main/java/com/team766/robot/OI.java index d7f65d05..a27909f0 100644 --- a/src/main/java/com/team766/robot/OI.java +++ b/src/main/java/com/team766/robot/OI.java @@ -1,11 +1,18 @@ package com.team766.robot; import com.team766.framework.Procedure; + +import java.io.IOException; + import com.team766.framework.Context; import com.team766.hal.JoystickReader; import com.team766.hal.RobotProvider; +import com.team766.hal.simulator.Camera; import com.team766.logging.Category; +import com.team766.robot.constants.InputConstants; import com.team766.robot.procedures.*; +import com.team766.simulator.interfaces.ElectricalDevice.Input; +import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.DriverStation; /** @@ -13,19 +20,23 @@ * interface to the code that allow control of the robot. */ public class OI extends Procedure { - private JoystickReader joystick0; - private JoystickReader joystick1; - private JoystickReader joystick2; + private final static int LEFT_STICK_X_AXIS = 0; + private final static int LEFT_STICK_Y_AXIS = 1; + private final static int RIGHT_STICK_X_AXIS = 2; + private final static int RIGHT_STICK_Y_AXIS = 3; + + private JoystickReader gamepad; public OI() { loggerCategory = Category.OPERATOR_INTERFACE; - joystick0 = RobotProvider.instance.getJoystick(0); - joystick1 = RobotProvider.instance.getJoystick(1); - joystick2 = RobotProvider.instance.getJoystick(2); + gamepad = RobotProvider.instance.getJoystick(0); } + + public void run(Context context) { + CameraServer.startAutomaticCapture(); + context.takeOwnership(Robot.drive); - public void run(final Context context) { while (true) { // wait for driver station data (and refresh it using the WPILib APIs) context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData()); @@ -33,6 +44,13 @@ public void run(final Context context) { // Add driver controls here - make sure to take/release ownership // of mechanisms when appropriate. + double leftX = gamepad.getAxis(LEFT_STICK_X_AXIS); + double leftY = gamepad.getAxis(LEFT_STICK_Y_AXIS); + double rightX = gamepad.getAxis(RIGHT_STICK_X_AXIS); + double rightY = gamepad.getAxis(RIGHT_STICK_Y_AXIS); + + log(/*"leftX: " + leftX + */ ", leftY: " + leftY + ", rightX: " + rightX /*+ ", rightY: " + rightY */); + Robot.drive.setArcadeDrivePower(-leftY, rightX); } } } diff --git a/src/main/java/com/team766/robot/Robot.java b/src/main/java/com/team766/robot/Robot.java index 89ef434d..ed781cf5 100644 --- a/src/main/java/com/team766/robot/Robot.java +++ b/src/main/java/com/team766/robot/Robot.java @@ -4,9 +4,11 @@ public class Robot { // Declare mechanisms here + public static Drive drive; public static void robotInit() { // Initialize mechanisms here + drive = new Drive(); } } diff --git a/src/main/java/com/team766/robot/constants/InputConstants.java b/src/main/java/com/team766/robot/constants/InputConstants.java new file mode 100644 index 00000000..401d4424 --- /dev/null +++ b/src/main/java/com/team766/robot/constants/InputConstants.java @@ -0,0 +1,24 @@ +package com.team766.robot.constants; + +/** + * Constants used for the Operator Interface, eg for joyticks, buttons, dials, etc. + * + * TODO: consider moving this into a config file. + */ +public final class InputConstants { + + //Joysticks + public static final int LEFT_JOYSTICK = 0; + public static final int RIGHT_JOYSTICK = 1; + public static final int CONTROL_PANEL = 2; + + //Navigation + public static final int AXIS_LEFT_RIGHT = 0; + public static final int AXIS_FORWARD_BACKWARD = 1; + public static final int AXIS_TWIST = 3; + // Joystick buttons + public static final int CROSS_DEFENSE = 1; + public static final int RESET_GYRO = 1; + public static final int RESET_CURRENT_POSITION = 11; + //Other buttons +} diff --git a/src/main/java/com/team766/robot/mechanisms/Drive.java b/src/main/java/com/team766/robot/mechanisms/Drive.java new file mode 100644 index 00000000..0715a129 --- /dev/null +++ b/src/main/java/com/team766/robot/mechanisms/Drive.java @@ -0,0 +1,31 @@ +package com.team766.robot.mechanisms; + +import com.team766.framework.Mechanism; +import com.team766.hal.RobotProvider; +import com.team766.hal.MotorController; + +public class Drive extends Mechanism { + private MotorController leftMotor; + private MotorController rightMotor; + + public Drive() { + leftMotor = RobotProvider.instance.getMotor("drive.leftMotor"); + rightMotor = RobotProvider.instance.getMotor("drive.rightMotor"); + } + + public void setArcadeDrivePower(double forward, double turn) { + checkContextOwnership(); + + // less is moar + forward *= 0.15; + turn *= 0.15; + + double leftMotorPower = turn + forward; + double rightMotorPower = -turn + forward; + + log("left: " + leftMotorPower + ", right: " + rightMotorPower); + + leftMotor.set(leftMotorPower); + rightMotor.set(rightMotorPower); + } +}