TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-15 21:22:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added experimental Hub led control, stubbed automous engines, added Robot#reportStatus for Robot to select 'worst' status

Browse Source
This commit is contained in:
Cyberarm
2023-01-22 21:24:41 -06:00
parent 3286b1ec57
commit 99163e0de7
8 changed files with 178 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

100
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java
View File

@@ -1,9 +1,9 @@
package org.timecrafters.minibots.cyberarm.chiron;
import static org.timecrafters.minibots.cyberarm.chiron.Robot.Status;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.Blinker;
import com.qualcomm.robotcore.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
@@ -21,18 +21,23 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Act
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
import org.timecrafters.minibots.cyberarm.chiron.tasks.FieldLocalizer;
import java.util.ArrayList;
import java.util.concurrent.TimeUnit;
public class Robot {
public final DcMotorEx backLeftDrive, frontRightDrive, frontLeftDrive, backRightDrive, arm;
public final ServoImplEx gripper, wrist;
public final IMU imu;
public final ColorSensor indicatorA, indicatorB;
public LynxModule expansionHub;
public final double imuAngleOffset;
public boolean wristManuallyControlled = false;
public boolean automaticAntiTipActive = false;
public boolean hardwareFault = false;
public Status status = Status.OKAY;
private Status status = Status.OKAY, lastStatus = Status.OKAY;
private ArrayList<Status> reportedStatuses = new ArrayList<>();
public enum ArmPosition {
COLLECT,
@@ -152,6 +157,20 @@ public class Robot {
imu.initialize(parameters);
// BulkRead from Hubs
for (LynxModule hub : engine.hardwareMap.getAll(LynxModule.class)) {
hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
if (!hub.isParent() && expansionHub == null) {
expansionHub = hub;
}
}
// Set LED pattern
if (expansionHub != null) {
expansionHub.setPattern(ledPatternOkay());
}
// INITIALIZE AFTER EVERYTHING ELSE to prevent use before set crashes
this.fieldLocalizer.setRobot(this);
this.fieldLocalizer.standardSetup();
@@ -162,6 +181,7 @@ public class Robot {
// STATUS
engine.telemetry.addLine("DATA");
engine.telemetry.addData(" Robot Status", status);
engine.telemetry.addData(" Hardware Fault", hardwareFault);
engine.telemetry.addLine();
@@ -280,10 +300,70 @@ public class Robot {
engine.telemetry.addLine();
}
public void update() {
status = Status.OKAY;
private ArrayList<Blinker.Step> ledPatternOkay() {
final ArrayList<Blinker.Step> steps = new ArrayList<>();
// TODO: Handle status priority; That is, store reported statuses and select the "worst" one as status
steps.add(new Blinker.Step(0x00aa00, 500, TimeUnit.MILLISECONDS));
steps.add(new Blinker.Step(0x00aa44, 500, TimeUnit.MILLISECONDS));
return steps;
}
private ArrayList<Blinker.Step> ledPatternMonitoring() {
final ArrayList<Blinker.Step> steps = new ArrayList<>();
steps.add(new Blinker.Step(0xaaaaaa, 500, TimeUnit.MILLISECONDS));
steps.add(new Blinker.Step(0x000000, 250, TimeUnit.MILLISECONDS));
return steps;
}
private ArrayList<Blinker.Step> ledPatternWarning() {
final ArrayList<Blinker.Step> steps = new ArrayList<>();
steps.add(new Blinker.Step(0xffff00, 500, TimeUnit.MILLISECONDS));
steps.add(new Blinker.Step(0xff8800, 500, TimeUnit.MILLISECONDS));
return steps;
}
private ArrayList<Blinker.Step> ledPatternDanger() {
final ArrayList<Blinker.Step> steps = new ArrayList<>();
steps.add(new Blinker.Step(0xff0000, 250, TimeUnit.MILLISECONDS));
steps.add(new Blinker.Step(0x000000, 100, TimeUnit.MILLISECONDS));
return steps;
}
public void reportStatus(Status status) {
reportedStatuses.add(status);
}
public void update() {
for (LynxModule hub : engine.hardwareMap.getAll(LynxModule.class)) {
hub.clearBulkCache();
}
status = Status.OKAY;
for (Status s : reportedStatuses) {
if (s.ordinal() > status.ordinal()) {
status = s;
}
}
reportedStatuses.clear();
if (status != lastStatus) {
lastStatus = status;
if (expansionHub != null) {
if (lastStatus == Status.OKAY) { expansionHub.setPattern(ledPatternOkay()); }
if (lastStatus == Status.MONITORING) { expansionHub.setPattern(ledPatternMonitoring()); }
if (lastStatus == Status.WARNING) { expansionHub.setPattern(ledPatternWarning()); }
if (lastStatus == Status.DANGER) { expansionHub.setPattern(ledPatternDanger()); }
}
}
automaticLEDStatus();
}
@@ -322,6 +402,8 @@ public class Robot {
}
}
public Status getStatus() { return status; }
public double getRadius() { return radius; }
public double getDiameter() { return diameter; }
@@ -334,11 +416,11 @@ public class Robot {
public int unitToTicks(DistanceUnit unit, double distance) {
double fI = (gearRatio * ticksPerRevolution) / (wheelRadius * 2 * Math.PI * (gearRatio * ticksPerRevolution) / (gearRatio * ticksPerRevolution));
double inches = unit.toInches(unit.fromUnit(unit, distance)); // NOTE: UNTESTED
double inches = unit.toInches(unit.fromUnit(unit, distance));
double ticks = fI * inches;
return (int)ticks; // NOTE: UNTESTED
return (int)ticks;
}
// For: Drive Wheels
@@ -346,7 +428,7 @@ public class Robot {
// Convert to inches, then to unit.
double inches = wheelRadius * 2 * Math.PI * ticks / (gearRatio * ticksPerRevolution);
return unit.fromUnit(DistanceUnit.INCH, inches); // NOTE: UNTESTED
return unit.fromUnit(DistanceUnit.INCH, inches);
}
// For: Arm

26
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousRedLeftSideEngine.java → TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousEngine.java
View File

@@ -1,25 +1,27 @@
package org.timecrafters.minibots.cyberarm.chiron.engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import org.timecrafters.minibots.cyberarm.chiron.Robot;
import org.timecrafters.minibots.cyberarm.chiron.tasks.FieldLocalizer;
@Autonomous(name = "CHIRON | RED Left Side", group = "CHIRON", preselectTeleOp = "CHIRON | TeleOp")
public class AutonomousRedLeftSideEngine extends CyberarmEngine {
private Robot robot;
private FieldLocalizer fieldLocalizer;
private TimeCraftersConfiguration configuration;
public class AutonomousEngine extends CyberarmEngine {
protected Robot robot;
protected FieldLocalizer fieldLocalizer;
protected TimeCraftersConfiguration configuration;
protected String configurationName = "CHIRON";
protected String actionsGroupName;
protected String tuningGroupName;
protected String tuningActionName;
@Override
public void setup() {
configuration = new TimeCraftersConfiguration("CHIRON");
configuration = new TimeCraftersConfiguration(configurationName);
fieldLocalizer = new FieldLocalizer(
configuration.variable("Autonomous", "Tuning_Red_LeftSide", "starting_position_x_in_inches").value(),
configuration.variable("Autonomous", "Tuning_Red_LeftSide", "starting_position_y_in_inches").value()
configuration.variable(tuningGroupName, tuningActionName, "starting_position_x_in_inches").value(),
configuration.variable(tuningGroupName, tuningActionName, "starting_position_y_in_inches").value()
);
robot = new Robot(
@@ -37,11 +39,13 @@ public class AutonomousRedLeftSideEngine extends CyberarmEngine {
"org.timecrafters.minibots.cyberarm.chiron.states.autonomous",
robot,
Robot.class,
"AutonomousRedLeftSide");
actionsGroupName);
}
@Override
public void loop() {
robot.update();
super.loop();
robot.standardTelemetry();

17
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueLeftSideEngine.java Normal file
View File

@@ -0,0 +1,17 @@
package org.timecrafters.minibots.cyberarm.chiron.engines.autonomous_engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine;
//@Autonomous(name = "CHIRON | BLUE Left Side", group = "CHIRON", preselectTeleOp = "CHIRON | TeleOp")
public class AutonomousBlueLeftSideEngine extends AutonomousEngine {
@Override
public void setup() {
actionsGroupName = "AutonomousBlueLeftSide";
tuningGroupName = "Autonomous";
tuningActionName = "Tuning_Blue_LeftSide";
super.setup();
}
}

17
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueRightSideEngine.java Normal file
View File

@@ -0,0 +1,17 @@
package org.timecrafters.minibots.cyberarm.chiron.engines.autonomous_engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine;
//@Autonomous(name = "CHIRON | BLUE Right Side", group = "CHIRON", preselectTeleOp = "CHIRON | TeleOp")
public class AutonomousBlueRightSideEngine extends AutonomousEngine {
@Override
public void setup() {
actionsGroupName = "AutonomousBlueRightSide";
tuningGroupName = "Autonomous";
tuningActionName = "Tuning_Blue_RightSide";
super.setup();
}
}

17
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedLeftSideEngine.java Normal file
View File

@@ -0,0 +1,17 @@
package org.timecrafters.minibots.cyberarm.chiron.engines.autonomous_engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine;
@Autonomous(name = "CHIRON | RED Left Side", group = "CHIRON", preselectTeleOp = "CHIRON | TeleOp")
public class AutonomousRedLeftSideEngine extends AutonomousEngine {
@Override
public void setup() {
actionsGroupName = "AutonomousRedLeftSide";
tuningGroupName = "Autonomous";
tuningActionName = "Tuning_Red_LeftSide";
super.setup();
}
}

17
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedRightSideEngine.java Normal file
View File

@@ -0,0 +1,17 @@
package org.timecrafters.minibots.cyberarm.chiron.engines.autonomous_engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine;
//@Autonomous(name = "CHIRON | RED Right Side", group = "CHIRON", preselectTeleOp = "CHIRON | TeleOp")
public class AutonomousRedRightSideEngine extends AutonomousEngine {
@Override
public void setup() {
actionsGroupName = "AutonomousRedRightSide";
tuningGroupName = "Autonomous";
tuningActionName = "Tuning_Red_RightSide";
super.setup();
}
}

6
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java
View File

@@ -46,7 +46,7 @@ public class ArmDriverControl extends CyberarmState {
return;
}
robot.status = Robot.Status.WARNING;
robot.reportStatus(Robot.Status.WARNING);
double stepInterval = robot.tuningConfig("arm_manual_step_interval").value();
int stepSize = robot.tuningConfig("arm_manual_step_size").value();
@@ -112,7 +112,7 @@ public class ArmDriverControl extends CyberarmState {
private void automaticHardwareMonitor() {
if (robot.hardwareFault) {
robot.status = Robot.Status.DANGER;
robot.reportStatus(Robot.Status.DANGER);
stopArm();
} else {
@@ -126,7 +126,7 @@ public class ArmDriverControl extends CyberarmState {
return;
}
robot.status = Robot.Status.WARNING;
robot.reportStatus(Robot.Status.WARNING);
switch (position) {
case COLLECT:

4
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java
View File

@@ -26,8 +26,6 @@ public class DrivetrainDriverControl extends CyberarmState {
@Override
public void exec() {
robot.update();
move();
automatics();
@@ -113,7 +111,7 @@ public class DrivetrainDriverControl extends CyberarmState {
// robot.hardwareFault = true;
if (robot.hardwareFault) {
robot.status = Robot.Status.DANGER;
robot.reportStatus(Robot.Status.DANGER);
stopDrive();
}