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

Commit f2ec15a

Browse files
rcahoonrcahoon
committed
First draft code for 2024 m-ayhem demo bots
1 parent 7697f09 commit f2ec15a

File tree

15 files changed

+483
-0
lines changed

15 files changed

+483
-0
lines changed

‎src/main/java/com/team766/robot/burro_arm/AutonomousModes.java‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
package com.team766.robot.burro_arm;
2+
3+
import com.team766.framework.AutonomousMode;
4+
import com.team766.robot.burro_arm.procedures.*;
5+
6+
public class AutonomousModes {
7+
public static final AutonomousMode[] AUTONOMOUS_MODES =
8+
new AutonomousMode[] {
9+
// Add autonomous modes here like this:
10+
// new AutonomousMode("NameOfAutonomousMode", () -> new MyAutonomousProcedure()),
11+
//
12+
// If your autonomous procedure has constructor arguments, you can
13+
// define one or more different autonomous modes with it like this:
14+
// new AutonomousMode("DriveFast", () -> new DriveStraight(1.0)),
15+
// new AutonomousMode("DriveSlow", () -> new DriveStraight(0.4)),
16+
17+
new AutonomousMode("DoNothing", () -> new DoNothing()),
18+
};
19+
}

‎src/main/java/com/team766/robot/burro_arm/OI.java‎

Lines changed: 60 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
package com.team766.robot.burro_arm;
2+
3+
import static com.team766.robot.burro_arm.constants.InputConstants.*;
4+
5+
import com.team766.framework.Context;
6+
import com.team766.framework.Procedure;
7+
import com.team766.hal.JoystickReader;
8+
import com.team766.hal.RobotProvider;
9+
import com.team766.logging.Category;
10+
import com.team766.robot.burro_arm.procedures.*;
11+
12+
/**
13+
* This class is the glue that binds the controls on the physical operator
14+
* interface to the code that allow control of the robot.
15+
*/
16+
public class OI extends Procedure {
17+
private JoystickReader joystick0;
18+
private JoystickReader joystick1;
19+
private JoystickReader joystick2;
20+
21+
public OI() {
22+
loggerCategory = Category.OPERATOR_INTERFACE;
23+
24+
joystick0 = RobotProvider.instance.getJoystick(0);
25+
joystick1 = RobotProvider.instance.getJoystick(1);
26+
joystick2 = RobotProvider.instance.getJoystick(2);
27+
}
28+
29+
public void run(final Context context) {
30+
context.takeOwnership(Robot.drive);
31+
context.takeOwnership(Robot.arm);
32+
context.takeOwnership(Robot.gripper);
33+
34+
while (true) {
35+
// wait for driver station data (and refresh it using the WPILib APIs)
36+
context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData());
37+
RobotProvider.instance.refreshDriverStationData();
38+
39+
// Add driver controls here - make sure to take/release ownership
40+
// of mechanisms when appropriate.
41+
42+
Robot.drive.drive(-joystick0.getAxis(1) * 0.5, -joystick0.getAxis(2) * 0.3);
43+
44+
if (joystick0.getButton(BUTTON_ARM_UP)) {
45+
Robot.arm.setAngle(Robot.arm.getAngle() + NUDGE_UP_INCREMENT);
46+
} else if (joystick0.getButton(BUTTON_ARM_DOWN)) {
47+
Robot.arm.setAngle(Robot.arm.getAngle() - NUDGE_DOWN_INCREMENT);
48+
}
49+
50+
if (joystick0.getButton(BUTTON_INTAKE)) {
51+
Robot.gripper.intake();
52+
} else if (joystick0.getButton(BUTTON_OUTTAKE)) {
53+
Robot.gripper.outtake();
54+
;
55+
} else {
56+
Robot.gripper.idle();
57+
}
58+
}
59+
}
60+
}

‎src/main/java/com/team766/robot/burro_arm/Robot.java‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
package com.team766.robot.burro_arm;
2+
3+
import com.team766.framework.AutonomousMode;
4+
import com.team766.framework.Procedure;
5+
import com.team766.hal.RobotConfigurator;
6+
import com.team766.robot.burro_arm.mechanisms.*;
7+
import com.team766.robot.common.mechanisms.BurroDrive;
8+
9+
public class Robot implements RobotConfigurator {
10+
// Declare mechanisms (as static fields) here
11+
public static BurroDrive drive;
12+
public static Arm arm;
13+
public static Gripper gripper;
14+
15+
@Override
16+
public void initializeMechanisms() {
17+
// Initialize mechanisms here
18+
drive = new BurroDrive();
19+
arm = new Arm();
20+
gripper = new Gripper();
21+
}
22+
23+
@Override
24+
public Procedure createOI() {
25+
return new OI();
26+
}
27+
28+
@Override
29+
public AutonomousMode[] getAutonomousModes() {
30+
return AutonomousModes.AUTONOMOUS_MODES;
31+
}
32+
}

‎src/main/java/com/team766/robot/burro_arm/constants/InputConstants.java‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
package com.team766.robot.burro_arm.constants;
2+
3+
/**
4+
* Constants used for the Operator Interface, eg for joyticks, buttons, dials, etc.
5+
*
6+
* Starter set of constants. Customize and update based on joystick and boxop controls.
7+
*/
8+
public final class InputConstants {
9+
// navigation
10+
public static final int AXIS_FORWARD_BACKWARD = 1;
11+
public static final int AXIS_TURN = 3;
12+
13+
// buttons
14+
public static final int BUTTON_ARM_UP = 5;
15+
public static final int BUTTON_ARM_DOWN = 6;
16+
public static final int BUTTON_INTAKE = 7;
17+
public static final int BUTTON_OUTTAKE = 8;
18+
19+
public static final double NUDGE_UP_INCREMENT = 5.0; // degrees
20+
public static final double NUDGE_DOWN_INCREMENT = 5.0; // degrees
21+
}

‎src/main/java/com/team766/robot/burro_arm/mechanisms/Arm.java‎

Lines changed: 67 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,67 @@
1+
package com.team766.robot.burro_arm.mechanisms;
2+
3+
import com.team766.config.ConfigFileReader;
4+
import com.team766.framework.Mechanism;
5+
import com.team766.hal.EncoderReader;
6+
import com.team766.hal.MotorController;
7+
import com.team766.hal.MotorController.ControlMode;
8+
import com.team766.hal.RobotProvider;
9+
import com.team766.library.RateLimiter;
10+
import com.team766.library.ValueProvider;
11+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
12+
13+
public class Arm extends Mechanism {
14+
private static final double ABSOLUTE_ENCODER_TO_ARM_ANGLE =
15+
(360. /*degrees per rotation*/) * (12. / 54. /*chain reduction*/);
16+
private static final double MOTOR_ROTATIONS_TO_ARM_ANGLE =
17+
ABSOLUTE_ENCODER_TO_ARM_ANGLE * (1. / (5. * 5. * 5.) /*planetary gearbox*/);
18+
19+
private final MotorController motor;
20+
private final EncoderReader absoluteEncoder;
21+
22+
private final ValueProvider<Double> absoluteEncoderOffset;
23+
private final RateLimiter dashboardRateLimiter = new RateLimiter(0.1);
24+
25+
private boolean initialized = false;
26+
27+
public Arm() {
28+
motor = RobotProvider.instance.getMotor("arm.Motor");
29+
absoluteEncoder = RobotProvider.instance.getEncoder("arm.AbsoluteEncoder");
30+
absoluteEncoderOffset = ConfigFileReader.instance.getDouble("arm.AbsoluteEncoderOffset");
31+
}
32+
33+
public void setPower(final double power) {
34+
checkContextOwnership();
35+
motor.set(power);
36+
}
37+
38+
public void setAngle(final double angle) {
39+
checkContextOwnership();
40+
41+
motor.set(ControlMode.Position, angle / MOTOR_ROTATIONS_TO_ARM_ANGLE);
42+
}
43+
44+
public double getAngle() {
45+
return motor.getSensorPosition() * MOTOR_ROTATIONS_TO_ARM_ANGLE;
46+
}
47+
48+
@Override
49+
public void run() {
50+
if (!initialized && absoluteEncoder.isConnected()) {
51+
final double absoluteEncoderPosition =
52+
Math.IEEEremainder(
53+
absoluteEncoder.getPosition() + absoluteEncoderOffset.get(), 1.0);
54+
SmartDashboard.putNumber(
55+
"[ARM] AbsoluteEncoder Init Position", absoluteEncoderPosition);
56+
motor.setSensorPosition(
57+
absoluteEncoderPosition
58+
* ABSOLUTE_ENCODER_TO_ARM_ANGLE
59+
/ MOTOR_ROTATIONS_TO_ARM_ANGLE);
60+
initialized = true;
61+
}
62+
63+
if (dashboardRateLimiter.next()) {
64+
SmartDashboard.putNumber("[Arm] Angle", getAngle());
65+
}
66+
}
67+
}

‎src/main/java/com/team766/robot/burro_arm/mechanisms/Gripper.java‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
package com.team766.robot.burro_arm.mechanisms;
2+
3+
import com.team766.config.ConfigFileReader;
4+
import com.team766.framework.Mechanism;
5+
import com.team766.hal.MotorController;
6+
import com.team766.hal.RobotProvider;
7+
8+
public class Gripper extends Mechanism {
9+
private final MotorController leftMotor;
10+
private final MotorController rightMotor;
11+
private final double intakePower = -ConfigFileReader.instance.getDouble("gripper.intakePower").valueOr(0.3);
12+
private final double outtakePower = ConfigFileReader.instance.getDouble("gripper.outtakePower").valueOr(0.1);
13+
private final double idlePower = -ConfigFileReader.instance.getDouble("gripper.idlePower").valueOr(0.05);
14+
15+
public Gripper() {
16+
leftMotor = RobotProvider.instance.getMotor("gripper.leftMotor");
17+
rightMotor = RobotProvider.instance.getMotor("gripper.rightMotor");
18+
}
19+
20+
public void setMotorPower(final double power) {
21+
checkContextOwnership();
22+
23+
leftMotor.set(power);
24+
rightMotor.set(power);
25+
}
26+
27+
public void intake() {
28+
setMotorPower(intakePower);
29+
}
30+
31+
public void outtake() {
32+
setMotorPower(outtakePower);
33+
}
34+
35+
public void idle() {
36+
setMotorPower(idlePower);
37+
}
38+
}

‎src/main/java/com/team766/robot/burro_arm/procedures/DoNothing.java‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
package com.team766.robot.burro_arm.procedures;
2+
3+
import com.team766.framework.Context;
4+
import com.team766.framework.Procedure;
5+
6+
public class DoNothing extends Procedure {
7+
8+
public void run(final Context context) {}
9+
}

‎src/main/java/com/team766/robot/burro_elevator/AutonomousModes.java‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
package com.team766.robot.burro_elevator;
2+
3+
import com.team766.framework.AutonomousMode;
4+
import com.team766.robot.burro_elevator.procedures.*;
5+
6+
public class AutonomousModes {
7+
public static final AutonomousMode[] AUTONOMOUS_MODES =
8+
new AutonomousMode[] {
9+
// Add autonomous modes here like this:
10+
// new AutonomousMode("NameOfAutonomousMode", () -> new MyAutonomousProcedure()),
11+
//
12+
// If your autonomous procedure has constructor arguments, you can
13+
// define one or more different autonomous modes with it like this:
14+
// new AutonomousMode("DriveFast", () -> new DriveStraight(1.0)),
15+
// new AutonomousMode("DriveSlow", () -> new DriveStraight(0.4)),
16+
17+
new AutonomousMode("DoNothing", () -> new DoNothing()),
18+
};
19+
}

‎src/main/java/com/team766/robot/burro_elevator/OI.java‎

Lines changed: 67 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,67 @@
1+
package com.team766.robot.burro_elevator;
2+
3+
import static com.team766.robot.burro_elevator.constants.InputConstants.*;
4+
5+
import com.team766.framework.Context;
6+
import com.team766.framework.Procedure;
7+
import com.team766.hal.JoystickReader;
8+
import com.team766.hal.RobotProvider;
9+
import com.team766.logging.Category;
10+
import com.team766.robot.burro_elevator.procedures.*;
11+
12+
/**
13+
* This class is the glue that binds the controls on the physical operator
14+
* interface to the code that allow control of the robot.
15+
*/
16+
public class OI extends Procedure {
17+
private JoystickReader joystick0;
18+
private JoystickReader joystick1;
19+
private JoystickReader joystick2;
20+
21+
public OI() {
22+
loggerCategory = Category.OPERATOR_INTERFACE;
23+
24+
joystick0 = RobotProvider.instance.getJoystick(0);
25+
joystick1 = RobotProvider.instance.getJoystick(1);
26+
joystick2 = RobotProvider.instance.getJoystick(2);
27+
}
28+
29+
public void run(final Context context) {
30+
context.takeOwnership(Robot.drive);
31+
context.takeOwnership(Robot.elevator);
32+
context.takeOwnership(Robot.gripper);
33+
34+
while (true) {
35+
// wait for driver station data (and refresh it using the WPILib APIs)
36+
context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData());
37+
RobotProvider.instance.refreshDriverStationData();
38+
39+
// Add driver controls here - make sure to take/release ownership
40+
// of mechanisms when appropriate.
41+
42+
Robot.drive.drive(
43+
-joystick0.getAxis(AXIS_FORWARD_BACKWARD) * 0.5, -joystick0.getAxis(AXIS_TURN) * 0.5);
44+
45+
if (joystick0.getButton(BUTTON_ELEVATOR_UP)) {
46+
Robot.elevator.setPosition(Robot.elevator.getPosition() + NUDGE_UP_INCREMENT);
47+
} else if (joystick0.getButton(BUTTON_ELEVATOR_DOWN)) {
48+
Robot.elevator.setPosition(Robot.elevator.getPosition() - NUDGE_DOWN_INCREMENT);
49+
}
50+
// if (joystick0.getButton(BUTTON_ELEVATOR_UP)) {
51+
// Robot.elevator.setPower(0.2);
52+
// } else if (joystick0.getButton(BUTTON_ELEVATOR_DOWN)) {
53+
// Robot.elevator.setPower(-0.2);
54+
// } else {
55+
// Robot.elevator.setPower(0);
56+
// }
57+
58+
if (joystick0.getButton(BUTTON_INTAKE)) {
59+
Robot.gripper.intake();
60+
} else if (joystick0.getButton(BUTTON_OUTTAKE)) {
61+
Robot.gripper.outtake();
62+
} else {
63+
Robot.gripper.idle();
64+
}
65+
}
66+
}
67+
}

‎src/main/java/com/team766/robot/burro_elevator/Robot.java‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
package com.team766.robot.burro_elevator;
2+
3+
import com.team766.framework.AutonomousMode;
4+
import com.team766.framework.Procedure;
5+
import com.team766.hal.RobotConfigurator;
6+
import com.team766.robot.burro_arm.mechanisms.Gripper;
7+
import com.team766.robot.burro_elevator.mechanisms.*;
8+
import com.team766.robot.common.mechanisms.BurroDrive;
9+
10+
public class Robot implements RobotConfigurator {
11+
// Declare mechanisms (as static fields) here
12+
public static BurroDrive drive;
13+
public static Elevator elevator;
14+
public static Gripper gripper;
15+
16+
@Override
17+
public void initializeMechanisms() {
18+
// Initialize mechanisms here
19+
drive = new BurroDrive();
20+
elevator = new Elevator();
21+
gripper = new Gripper();
22+
}
23+
24+
@Override
25+
public Procedure createOI() {
26+
return new OI();
27+
}
28+
29+
@Override
30+
public AutonomousMode[] getAutonomousModes() {
31+
return AutonomousModes.AUTONOMOUS_MODES;
32+
}
33+
}

0 commit comments

Comments
 (0)