Skip to content
This repository has been archived by the owner on Jan 13, 2025. It is now read-only.

Commit

Permalink
MF3 demo bots (v1)
Browse files Browse the repository at this point in the history
  • Loading branch information
@rcahoon
rcahoon committed Dec 14, 2024
1 parent a4f1517 commit 1cea4ae
Show file tree
Hide file tree
Showing 10 changed files with 362 additions and 160 deletions.
85 changes: 49 additions & 36 deletions src/main/java/com/team766/robot/burro_arm/OI.java
View file
Original file line number Diff line number Diff line change
@@ -1,60 +1,73 @@
package com.team766.robot.burro_arm;

import static com.team766.framework.RulePersistence.ONCE;
import static com.team766.framework.RulePersistence.ONCE_AND_HOLD;
import static com.team766.framework.RulePersistence.REPEATEDLY;
import static com.team766.robot.burro_arm.constants.InputConstants.*;

import com.team766.framework.Context;
import com.team766.framework.Procedure;
import com.team766.framework.Rule;
import com.team766.framework.RuleEngine;
import com.team766.hal.JoystickReader;
import com.team766.hal.RobotProvider;
import com.team766.logging.Category;
import com.team766.robot.burro_arm.mechanisms.*;
import com.team766.robot.burro_arm.procedures.*;
import com.team766.robot.common.mechanisms.BurroDrive;
import java.util.Set;

/**
* This class is the glue that binds the controls on the physical operator
* interface to the code that allow control of the robot.
*/
public class OI extends Procedure {
public class OI extends RuleEngine {
private JoystickReader joystick0;
private JoystickReader joystick1;
private JoystickReader joystick2;

public OI() {
loggerCategory = Category.OPERATOR_INTERFACE;
@Override
public Category getLoggerCategory() {
return Category.OPERATOR_INTERFACE;
}

public OI(BurroDrive drive, Arm arm, Gripper gripper) {
joystick0 = RobotProvider.instance.getJoystick(0);
joystick1 = RobotProvider.instance.getJoystick(1);
joystick2 = RobotProvider.instance.getJoystick(2);
}

public void run(final Context context) {
context.takeOwnership(Robot.drive);
context.takeOwnership(Robot.arm);
context.takeOwnership(Robot.gripper);

while (true) {
// wait for driver station data (and refresh it using the WPILib APIs)
context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData());
RobotProvider.instance.refreshDriverStationData();

// Add driver controls here - make sure to take/release ownership
// of mechanisms when appropriate.

Robot.drive.drive(-joystick0.getAxis(1) * 0.5, -joystick0.getAxis(2) * 0.3);

if (joystick0.getButton(BUTTON_ARM_UP)) {
Robot.arm.setAngle(Robot.arm.getAngle() + NUDGE_UP_INCREMENT);
} else if (joystick0.getButton(BUTTON_ARM_DOWN)) {
Robot.arm.setAngle(Robot.arm.getAngle() - NUDGE_DOWN_INCREMENT);
}

if (joystick0.getButton(BUTTON_INTAKE)) {
Robot.gripper.intake();
} else if (joystick0.getButton(BUTTON_OUTTAKE)) {
Robot.gripper.outtake();
;
} else {
Robot.gripper.idle();
}
}
// Add driver controls here.

addRule(
Rule.create("Drive Robot", () -> true)
.withOnTriggeringProcedure(
REPEATEDLY,
Set.of(drive),
() -> {
drive.setRequest(
new BurroDrive.ArcadeDrive(
-joystick0.getAxis(AXIS_FORWARD_BACKWARD) * 0.5,
-joystick0.getAxis(AXIS_TURN) * 0.3));
}));

addRule(
Rule.create("Arm Up", () -> joystick0.getButton(BUTTON_ARM_UP))
.withOnTriggeringProcedure(
ONCE, Set.of(arm), () -> arm.setRequest(Arm.makeNudgeUp())));
addRule(
Rule.create("Arm Down", () -> joystick0.getButton(BUTTON_ARM_DOWN))
.withOnTriggeringProcedure(
ONCE, Set.of(arm), () -> arm.setRequest(Arm.makeNudgeDown())));

addRule(
Rule.create("Intake", () -> joystick0.getButton(BUTTON_INTAKE))
.withOnTriggeringProcedure(
ONCE_AND_HOLD,
Set.of(gripper),
() -> gripper.setRequest(new Gripper.Intake())));
addRule(
Rule.create("Outtake", () -> joystick0.getButton(BUTTON_OUTTAKE))
.withOnTriggeringProcedure(
ONCE_AND_HOLD,
Set.of(gripper),
() -> gripper.setRequest(new Gripper.Outtake())));
}
}
17 changes: 11 additions & 6 deletions src/main/java/com/team766/robot/burro_arm/Robot.java
View file
Original file line number Diff line number Diff line change
@@ -1,16 +1,16 @@
package com.team766.robot.burro_arm;

import com.team766.framework.AutonomousMode;
import com.team766.framework.Procedure;
import com.team766.framework.RuleEngine;
import com.team766.hal.RobotConfigurator;
import com.team766.robot.burro_arm.mechanisms.*;
import com.team766.robot.common.mechanisms.BurroDrive;

public class Robot implements RobotConfigurator {
// Declare mechanisms (as static fields) here
public static BurroDrive drive;
public static Arm arm;
public static Gripper gripper;
private BurroDrive drive;
private Arm arm;
private Gripper gripper;

@Override
public void initializeMechanisms() {
Expand All @@ -21,8 +21,13 @@ public void initializeMechanisms() {
}

@Override
public Procedure createOI() {
return new OI();
public RuleEngine createOI() {
return new OI(drive, arm, gripper);
}

@Override
public RuleEngine createLights() {
return null;
}

@Override
Expand Down
3 changes: 0 additions & 3 deletions src/main/java/com/team766/robot/burro_arm/constants/InputConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,4 @@ public final class InputConstants {
public static final int BUTTON_ARM_DOWN = 6;
public static final int BUTTON_INTAKE = 7;
public static final int BUTTON_OUTTAKE = 8;

public static final double NUDGE_UP_INCREMENT = 5.0; // degrees
public static final double NUDGE_DOWN_INCREMENT = 5.0; // degrees
}
82 changes: 66 additions & 16 deletions src/main/java/com/team766/robot/burro_arm/mechanisms/Arm.java
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,12 @@
package com.team766.robot.burro_arm.mechanisms;

import static com.team766.framework.Conditions.checkForStatusWith;
import static com.team766.framework.StatusBus.getStatusOrThrow;

import com.team766.config.ConfigFileReader;
import com.team766.framework.Mechanism;
import com.team766.framework.Request;
import com.team766.framework.Status;
import com.team766.hal.EncoderReader;
import com.team766.hal.MotorController.ControlMode;
import com.team766.hal.RobotProvider;
Expand All @@ -10,7 +15,47 @@
import com.team766.library.ValueProvider;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class Arm extends Mechanism {
public class Arm extends Mechanism<Arm.ArmRequest, Arm.ArmStatus> {
public record ArmStatus(double angle) implements Status {}

public sealed interface ArmRequest extends Request {}

public record SetPower(double power) implements ArmRequest {
@Override
public boolean isDone() {
return true;
}
}

public record SetAngle(double angle) implements ArmRequest {
@Override
public boolean isDone() {
// TODO: also consider velocity
return checkForStatusWith(
ArmStatus.class, s -> Math.abs(s.angle - angle) < ANGLE_TOLERANCE);
}
}

public static ArmRequest makeHoldPosition() {
final double currentAngle = getStatusOrThrow(ArmStatus.class).angle();
return new SetAngle(currentAngle);
}

public static ArmRequest makeNudgeUp() {
final double currentAngle = getStatusOrThrow(ArmStatus.class).angle();
return new SetAngle(currentAngle + NUDGE_UP_INCREMENT);
}

public static ArmRequest makeNudgeDown() {
final double currentAngle = getStatusOrThrow(ArmStatus.class).angle();
return new SetAngle(currentAngle - NUDGE_DOWN_INCREMENT);
}

private static final double NUDGE_UP_INCREMENT = 5.0; // degrees
private static final double NUDGE_DOWN_INCREMENT = 5.0; // degrees

private static final double ANGLE_TOLERANCE = 3; // degrees

private static final double ABSOLUTE_ENCODER_TO_ARM_ANGLE =
(360. /*degrees per rotation*/) * (12. / 54. /*chain reduction*/);
private static final double MOTOR_ROTATIONS_TO_ARM_ANGLE =
Expand All @@ -25,29 +70,24 @@ public class Arm extends Mechanism {
private boolean initialized = false;

public Arm() {
motor = (CANSparkMaxMotorController)RobotProvider.instance.getMotor("arm.Motor");
motor = (CANSparkMaxMotorController) RobotProvider.instance.getMotor("arm.Motor");
motor.setSmartCurrentLimit(5, 80, 200);
absoluteEncoder = RobotProvider.instance.getEncoder("arm.AbsoluteEncoder");
absoluteEncoderOffset = ConfigFileReader.instance.getDouble("arm.AbsoluteEncoderOffset");
}

public void setPower(final double power) {
checkContextOwnership();
motor.set(power);
}

public void setAngle(final double angle) {
checkContextOwnership();

motor.set(ControlMode.Position, angle / MOTOR_ROTATIONS_TO_ARM_ANGLE);
@Override
protected ArmRequest getInitialRequest() {
return new SetPower(0);
}

public double getAngle() {
return motor.getSensorPosition() * MOTOR_ROTATIONS_TO_ARM_ANGLE;
@Override
protected ArmRequest getIdleRequest() {
return makeHoldPosition();
}

@Override
public void run() {
public ArmStatus run(ArmRequest request, boolean isRequestNew) {
if (!initialized && absoluteEncoder.isConnected()) {
final double absoluteEncoderPosition =
Math.IEEEremainder(
Expand All @@ -61,8 +101,18 @@ public void run() {
initialized = true;
}

if (dashboardRateLimiter.next()) {
SmartDashboard.putNumber("[Arm] Angle", getAngle());
final ArmStatus status =
new ArmStatus(motor.getSensorPosition() * MOTOR_ROTATIONS_TO_ARM_ANGLE);

switch (request) {
case SetPower g -> {
motor.set(g.power);
}
case SetAngle g -> {
motor.set(ControlMode.Position, g.angle / MOTOR_ROTATIONS_TO_ARM_ANGLE);
}
}

return status;
}
}
72 changes: 57 additions & 15 deletions src/main/java/com/team766/robot/burro_arm/mechanisms/Gripper.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,37 +2,79 @@

import com.team766.config.ConfigFileReader;
import com.team766.framework.Mechanism;
import com.team766.framework.Request;
import com.team766.framework.Status;
import com.team766.hal.MotorController;
import com.team766.hal.RobotProvider;

public class Gripper extends Mechanism {
public class Gripper extends Mechanism<Gripper.GripperRequest, Gripper.GripperStatus> {
public record GripperStatus() implements Status {}

public sealed interface GripperRequest extends Request {}

public record Idle() implements GripperRequest {
@Override
public boolean isDone() {
return true;
}
}

public record Intake() implements GripperRequest {
@Override
public boolean isDone() {
return true;
}
}

public record Outtake() implements GripperRequest {
@Override
public boolean isDone() {
return true;
}
}

private final MotorController leftMotor;
private final MotorController rightMotor;
private final double intakePower = -ConfigFileReader.instance.getDouble("gripper.intakePower").valueOr(0.3);
private final double outtakePower = ConfigFileReader.instance.getDouble("gripper.outtakePower").valueOr(0.1);
private final double idlePower = -ConfigFileReader.instance.getDouble("gripper.idlePower").valueOr(0.05);
private final double intakePower =
-ConfigFileReader.instance.getDouble("gripper.intakePower").valueOr(0.3);
private final double outtakePower =
ConfigFileReader.instance.getDouble("gripper.outtakePower").valueOr(0.1);
private final double idlePower =
-ConfigFileReader.instance.getDouble("gripper.idlePower").valueOr(0.05);

public Gripper() {
leftMotor = RobotProvider.instance.getMotor("gripper.leftMotor");
rightMotor = RobotProvider.instance.getMotor("gripper.rightMotor");
}

public void setMotorPower(final double power) {
checkContextOwnership();

leftMotor.set(power);
rightMotor.set(power);
@Override
protected GripperRequest getInitialRequest() {
return new Idle();
}

public void intake() {
setMotorPower(intakePower);
@Override
protected GripperRequest getIdleRequest() {
return new Idle();
}

public void outtake() {
setMotorPower(outtakePower);
@Override
protected GripperStatus run(GripperRequest request, boolean isRequestNew) {
switch (request) {
case Idle g -> {
setMotorPower(idlePower);
}
case Intake g -> {
setMotorPower(intakePower);
}
case Outtake g -> {
setMotorPower(outtakePower);
}
}
return new GripperStatus();
}

public void idle() {
setMotorPower(idlePower);
private void setMotorPower(final double power) {
leftMotor.set(power);
rightMotor.set(power);
}
}
Loading

0 comments on commit 1cea4ae

Please sign in to comment.