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

Migrate robot code to MFv3 #150

Draft
wants to merge 1 commit into
base: rcahoon/mf3-mechanism-status
Choose a base branch
from
Draft

Migrate robot code to MFv3 #150

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
8 changes: 0 additions & 8 deletions src/main/java/com/team766/ViSIONbase/AprilTagGeneralUncheckedException.java
View file
Open in desktop

This file was deleted.

10 changes: 10 additions & 0 deletions src/main/java/com/team766/framework3/Superstructure.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,16 @@
import java.util.ArrayList;
import java.util.Objects;

/**
* A Superstructure coordinates the behavior of several Mechanisms which could interfere with one
* another if not commanded properly (e.g. they could collide with each other, or some other part
* of the robot, or the floor, etc).
*
* A Mechanism in a Superstructure cannot be reserved individually by Procedures (Procedures
* should reserve the entire Superstructure) and cannot have an Idle request. Only the
* Superstructure should set requests on its constituent Mechanisms (in its
* {@link #run(R, boolean)} method).
*/
public abstract class Superstructure<R extends Request, S extends Record & Status>
extends Mechanism<R, S> {
private ArrayList<Mechanism<?, ?>> submechanisms = new ArrayList<>();
Expand Down
183 changes: 91 additions & 92 deletions src/main/java/com/team766/robot/common/DriverOI.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,109 +1,108 @@
package com.team766.robot.common;

import com.team766.framework.Context;
import com.team766.framework.OIFragment;
import static com.team766.framework3.RulePersistence.*;

import com.team766.framework3.Conditions;
import com.team766.framework3.Rule;
import com.team766.framework3.RuleEngine;
import com.team766.hal.JoystickReader;
import com.team766.robot.common.constants.ControlConstants;
import com.team766.robot.common.constants.InputConstants;
import com.team766.robot.common.mechanisms.SwerveDrive;
import java.util.Set;

public class DriverOI extends OIFragment {

protected static final double FINE_DRIVING_COEFFICIENT = 0.25;

protected final SwerveDrive drive;
protected final JoystickReader leftJoystick;
protected final JoystickReader rightJoystick;
protected double rightJoystickY = 0;
protected double leftJoystickX = 0;
protected double leftJoystickY = 0;
protected boolean isCross = false;

private final OICondition movingJoysticks;

public DriverOI(SwerveDrive drive, JoystickReader leftJoystick, JoystickReader rightJoystick) {
this.drive = drive;
this.leftJoystick = leftJoystick;
this.rightJoystick = rightJoystick;

movingJoysticks =
new OICondition(
() ->
!isCross
&& Math.abs(leftJoystickX)
+ Math.abs(leftJoystickY)
+ Math.abs(rightJoystickY)
> 0);
}

public void handleOI(Context context) {

// Negative because forward is negative in driver station
leftJoystickX =
-createJoystickDeadzone(leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD))
* ControlConstants.MAX_POSITIONAL_VELOCITY; // For fwd/rv
// Negative because left is negative in driver station
leftJoystickY =
-createJoystickDeadzone(leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT))
* ControlConstants.MAX_POSITIONAL_VELOCITY; // For left/right
// Negative because left is negative in driver station
rightJoystickY =
-createJoystickDeadzone(rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT))
* ControlConstants.MAX_ROTATIONAL_VELOCITY; // For steer
public class DriverOI {
public DriverOI(
RuleEngine oi,
JoystickReader leftJoystick,
JoystickReader rightJoystick,
SwerveDrive drive) {
leftJoystick.setAllAxisDeadzone(ControlConstants.JOYSTICK_DEADZONE);
rightJoystick.setAllAxisDeadzone(ControlConstants.JOYSTICK_DEADZONE);

if (leftJoystick.getButtonPressed(InputConstants.BUTTON_RESET_GYRO)) {
drive.resetGyro();
}

if (leftJoystick.getButtonPressed(InputConstants.BUTTON_RESET_POS)) {
drive.resetCurrentPosition();
}
oi.addRule(
Rule.create(
"Reset Gyro",
() -> leftJoystick.getButton(InputConstants.BUTTON_RESET_GYRO))
.withOnTriggeringProcedure(ONCE, Set.of(drive), () -> drive.resetGyro()));
oi.addRule(
Rule.create(
"Reset Pos",
() -> leftJoystick.getButton(InputConstants.BUTTON_RESET_POS))
.withOnTriggeringProcedure(
ONCE, Set.of(drive), () -> drive.resetCurrentPosition()));

// Sets the wheels to the cross position if the cross button is pressed
if (rightJoystick.getButtonPressed(InputConstants.BUTTON_CROSS_WHEELS)) {
if (!isCross) {
context.takeOwnership(drive);
drive.stopDrive();
drive.setCross();
}
isCross = !isCross;
}
oi.addRule(
Rule.create(
"Cross Wheels",
new Conditions.Toggle(
() ->
rightJoystick.getButton(
InputConstants.BUTTON_CROSS_WHEELS)))
.withOnTriggeringProcedure(
ONCE_AND_HOLD,
Set.of(drive),
() -> drive.setRequest(new SwerveDrive.SetCross())));

// Moves the robot if there are joystick inputs
if (movingJoysticks.isTriggering()) {
double drivingCoefficient = 1;
// If a button is pressed, drive is just fine adjustment
if (rightJoystick.getButton(InputConstants.BUTTON_FINE_DRIVING)) {
drivingCoefficient = FINE_DRIVING_COEFFICIENT;
}
context.takeOwnership(drive);
drive.controlFieldOriented(
(drivingCoefficient
* curvedJoystickPower(
leftJoystickX, ControlConstants.TRANSLATIONAL_CURVE_POWER)),
(drivingCoefficient
* curvedJoystickPower(
leftJoystickY, ControlConstants.TRANSLATIONAL_CURVE_POWER)),
(drivingCoefficient
* curvedJoystickPower(
rightJoystickY, ControlConstants.ROTATIONAL_CURVE_POWER)));
} else {
context.takeOwnership(drive);
drive.stopDrive();
drive.setCross();
}
}

/**
* Helper method to ignore joystick values below JOYSTICK_DEADZONE
* @param joystickValue the value to trim
* @return the trimmed joystick value
*/
private double createJoystickDeadzone(double joystickValue) {
return Math.abs(joystickValue) > ControlConstants.JOYSTICK_DEADZONE ? joystickValue : 0;
oi.addRule(
Rule.create(
"Joysticks moved",
() ->
leftJoystick.isAxisMoved(
InputConstants.AXIS_FORWARD_BACKWARD)
|| leftJoystick.isAxisMoved(
InputConstants.AXIS_LEFT_RIGHT)
|| rightJoystick.isAxisMoved(
InputConstants.AXIS_LEFT_RIGHT))
.withOnTriggeringProcedure(
REPEATEDLY,
Set.of(drive),
() -> {
// For fwd/rv
// Negative because forward is negative in driver station
final double leftJoystickX =
-leftJoystick.getAxis(
InputConstants.AXIS_FORWARD_BACKWARD)
* ControlConstants.MAX_TRANSLATIONAL_VELOCITY;
// For left/right
// Negative because left is negative in driver station
final double leftJoystickY =
-leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)
* ControlConstants.MAX_TRANSLATIONAL_VELOCITY;
// For steer
// Negative because left is negative in driver station
final double rightJoystickY =
-rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)
* ControlConstants.MAX_ROTATIONAL_VELOCITY;
// If a button is pressed, drive is just fine adjustment
final double drivingCoefficient =
rightJoystick.getButton(
InputConstants.BUTTON_FINE_DRIVING)
? ControlConstants.FINE_DRIVING_COEFFICIENT
: 1;
drive.setRequest(
new SwerveDrive.FieldOrientedVelocity(
(drivingCoefficient
* curvedJoystickPower(
leftJoystickX,
ControlConstants
.TRANSLATIONAL_CURVE_POWER)),
(drivingCoefficient
* curvedJoystickPower(
leftJoystickY,
ControlConstants
.TRANSLATIONAL_CURVE_POWER)),
(drivingCoefficient
* curvedJoystickPower(
rightJoystickY,
ControlConstants
.ROTATIONAL_CURVE_POWER))));
}));
}

private double curvedJoystickPower(double value, double power) {
private static double curvedJoystickPower(double value, double power) {
return Math.signum(value) * Math.pow(Math.abs(value), power);
}
}
20 changes: 17 additions & 3 deletions src/main/java/com/team766/robot/common/constants/ControlConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,14 +15,28 @@ public class ControlConstants {
public static final double ROTATIONAL_CURVE_POWER = 1.0;

/**
* Positional velocity of robot that max joystick power controls in m/s
* Translational velocity of robot that max joystick power controls in m/s
*/
public static final double MAX_POSITIONAL_VELOCITY = 5.0;
public static final double MAX_TRANSLATIONAL_VELOCITY = 5.0;

/**
* m/s
*/
public static final double AT_TRANSLATIONAL_SPEED_THRESHOLD =
0.5; // TODO(MF3): Find actual value

/**
* Rotational velocity of robot that max joystick power controls in rad/s
*/
public static final double MAX_ROTATIONAL_VELOCITY = 4.0;

public static final double DEFAULT_ROTATION_THRESHOLD = 0.40;
/**
* degrees
*/
public static final double AT_ROTATIONAL_ANGLE_THRESHOLD = 3.0; // TODO(MF3): Find actual value

/**
* degrees/second
*/
public static final double AT_ROTATIONAL_SPEED_THRESHOLD = 3.0; // TODO(MF3): Find actual value
}
Loading
Loading