TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-12 23:32:36 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
NerdyBirdy460
2024-02-13 20:50:54 -06:00
parent 3424549d61 255f75e3d4
commit 810914dd77
23 changed files with 531 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/Localizer.java
View File

@@ -13,16 +13,18 @@ import dev.cyberarm.engine.V2.Utilities;
public class Localizer {
private final RedCrabMinibot robot;
private double rawX = 0, rawY = 0, rawR = 0, offsetX = 0, offsetY = 0;
private final double trackWidthMM = 387.35, forwardOffsetMM = 133.35, wheelDiameterMM = 90.0;
private double trackWidthMM = 387.35, forwardOffsetMM = 133.35, wheelDiameterMM = 90.0;
private final int encoderTicksPerRevolution = 8192;
private final double encoderGearRatio = 1;
private double lastEncoderXLeftMM, lastEncoderXRightMM, lastEncoderYCenterMM;
// private double xDeltaMultiplier = 0.87012987, yDeltaMultiplier = 0.25;
private double xPosMultiplier = 0.675956739, yPosMultiplier = 0.941867531;
private double xPosMultiplier = 0.675956739, yPosMultiplier = 0.675956739;
private HolonomicOdometry odometry;
public Localizer(RedCrabMinibot robot) {
this.robot = robot;
loadConfigConstants();
// Preset last encoder to current location to not require resetting encoders, ever. (🤞)
this.lastEncoderXLeftMM = ticksToMM(robot.deadWheelXLeft.getCurrentPosition());
this.lastEncoderXRightMM = ticksToMM(robot.deadWheelXRight.getCurrentPosition());
@@ -37,6 +39,22 @@ public class Localizer {
);
}
public void loadConfigConstants() {
// trackWidthMM = 387.35;
// forwardOffsetMM = 133.35;
// wheelDiameterMM = 90.0;
//
// xPosMultiplier = 0.675956739;
// yPosMultiplier = 0.675956739;
trackWidthMM = robot.config.variable("Robot", "Localizer", "track_width_mm").value();
forwardOffsetMM = robot.config.variable("Robot", "Localizer", "forward_offset_mm").value();
wheelDiameterMM = robot.config.variable("Robot", "Localizer", "wheel_diameter_mm").value();
xPosMultiplier = robot.config.variable("Robot", "Localizer", "x_position_multiplier").value();
yPosMultiplier = robot.config.variable("Robot", "Localizer", "y_position_multiplier").value();
}
public void reset() {
robot.resetDeadWheels();
@@ -104,7 +122,7 @@ public class Localizer {
}
public double yMM() {
return odometry.getPose().getY() * xPosMultiplier + offsetY; //rawY;
return odometry.getPose().getY() * yPosMultiplier + offsetY; //rawY;
}
public double xIn() {

204
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java
View File

@@ -22,6 +22,8 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
import java.util.ArrayList;
import dev.cyberarm.drivers.EncoderCustomKB2040;
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.engine.V2.Utilities;
@@ -87,8 +89,15 @@ public class RedCrabMinibot {
public final Servo leftClaw, rightClaw, clawWrist, droneLatch, hookArm;
public final DcMotorEx deadWheelXLeft, deadWheelXRight;
public final EncoderCustomKB2040 deadWheelYCenter;
public final DigitalChannel greenLED, redLED;
public final boolean LED_OFF = true, LED_ON = false;
public final DigitalChannel ledTopGreen, ledTopRed, ledRail0Green, ledRail0Red, ledRail1Green, ledRail1Red, ledRail2Green, ledRail2Red,
ledRail3Green, ledRail3Red;
public final ArrayList<DigitalChannel> railGreenLEDs = new ArrayList<>(), railRedLEDs = new ArrayList<>();
public int ledChaserIndex = 0;
public final long ledChaserIntervalMS;
public long lastLedChaserTimeMS;
public final boolean LED_OFF = true;
public final boolean LED_ON = false;
public boolean ledChaseUp = true;
final CyberarmEngine engine;
public final boolean autonomous;
@@ -252,13 +261,51 @@ public class RedCrabMinibot {
resetDeadWheels();
/// LED(s) ///
greenLED = engine.hardwareMap.get(DigitalChannel.class, "led_green"); // GREEN LED NOT WORKING
redLED = engine.hardwareMap.get(DigitalChannel.class, "led_red");
ledTopGreen = engine.hardwareMap.get(DigitalChannel.class, "led_top_green");
ledTopRed = engine.hardwareMap.get(DigitalChannel.class, "led_top_red");
// RED and GREEN are swapped for the 'new' LEDs, and I'm to lazy to fix the robot config
ledRail0Green = engine.hardwareMap.get(DigitalChannel.class, "led_rail_0_red");
ledRail0Red = engine.hardwareMap.get(DigitalChannel.class, "led_rail_0_green");
ledRail1Green = engine.hardwareMap.get(DigitalChannel.class, "led_rail_1_red");
ledRail1Red = engine.hardwareMap.get(DigitalChannel.class, "led_rail_1_green");
ledRail2Green = engine.hardwareMap.get(DigitalChannel.class, "led_rail_2_red");
ledRail2Red = engine.hardwareMap.get(DigitalChannel.class, "led_rail_2_green");
ledRail3Green = engine.hardwareMap.get(DigitalChannel.class, "led_rail_3_red");
ledRail3Red = engine.hardwareMap.get(DigitalChannel.class, "led_rail_3_green");
greenLED.setMode(DigitalChannel.Mode.OUTPUT);
redLED.setMode(DigitalChannel.Mode.OUTPUT);
greenLED.setState(LED_OFF);
redLED.setState(LED_ON);
railGreenLEDs.add(ledRail0Green);
railGreenLEDs.add(ledRail1Green);
railGreenLEDs.add(ledRail2Green);
railGreenLEDs.add(ledRail3Green);
railRedLEDs.add(ledRail0Red);
railRedLEDs.add(ledRail1Red);
railRedLEDs.add(ledRail2Red);
railRedLEDs.add(ledRail3Red);
ledTopGreen.setMode(DigitalChannel.Mode.OUTPUT);
ledTopRed.setMode(DigitalChannel.Mode.OUTPUT);
ledRail0Green.setMode(DigitalChannel.Mode.OUTPUT);
ledRail0Red.setMode(DigitalChannel.Mode.OUTPUT);
ledRail1Green.setMode(DigitalChannel.Mode.OUTPUT);
ledRail1Red.setMode(DigitalChannel.Mode.OUTPUT);
ledRail2Green.setMode(DigitalChannel.Mode.OUTPUT);
ledRail2Red.setMode(DigitalChannel.Mode.OUTPUT);
ledRail3Green.setMode(DigitalChannel.Mode.OUTPUT);
ledRail3Red.setMode(DigitalChannel.Mode.OUTPUT);
/// Turn off green LEDs and turn red ones on
ledTopGreen.setState(LED_OFF);
ledTopRed.setState(LED_ON);
for (DigitalChannel led : railGreenLEDs) {
led.setState(LED_OFF);
}
for (DigitalChannel led : railRedLEDs) {
led.setState(LED_ON);
}
ledChaserIntervalMS = 250;
lastLedChaserTimeMS = System.currentTimeMillis();
// Bulk read from hubs
Utilities.hubsBulkReadMode(engine.hardwareMap, LynxModule.BulkCachingMode.MANUAL);
@@ -274,8 +321,16 @@ public class RedCrabMinibot {
public void shutdown() {
// Prevent LED(s) from being on while idle
greenLED.setMode(DigitalChannel.Mode.INPUT);
redLED.setMode(DigitalChannel.Mode.INPUT);
ledTopGreen.setMode(DigitalChannel.Mode.INPUT);
ledTopRed.setMode(DigitalChannel.Mode.INPUT);
ledRail0Green.setMode(DigitalChannel.Mode.INPUT);
ledRail0Red.setMode(DigitalChannel.Mode.INPUT);
ledRail1Green.setMode(DigitalChannel.Mode.INPUT);
ledRail1Red.setMode(DigitalChannel.Mode.INPUT);
ledRail2Green.setMode(DigitalChannel.Mode.INPUT);
ledRail2Red.setMode(DigitalChannel.Mode.INPUT);
ledRail3Green.setMode(DigitalChannel.Mode.INPUT);
ledRail3Red.setMode(DigitalChannel.Mode.INPUT);
}
public void reloadConfig() {
@@ -331,6 +386,8 @@ public class RedCrabMinibot {
RedCrabMinibot.DRONE_LATCH_INITIAL_POSITION = config.variable("Robot", "DroneLauncher_Tuning", "initial_position").value();
RedCrabMinibot.DRONE_LATCH_LAUNCH_POSITION = config.variable("Robot", "DroneLauncher_Tuning", "launch_position").value();
RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS = config.variable("Robot", "DroneLauncher_Tuning", "launch_confirmation_time_ms").value();
localizer.loadConfigConstants();
}
public void resetDeadWheels() {
@@ -571,4 +628,131 @@ public class RedCrabMinibot {
return Utilities.isBetween(distanceMM, travelledDistanceMM - toleranceMM, travelledDistanceMM + toleranceMM);
}
public void ledController() {
if (autonomous) {
if (engine.runTime() < 20_000.0) { // KEEP CALM and CARRY ON
ledTopRed.setState(LED_OFF);
ledTopGreen.setState(LED_ON);
ledAnimateChaser(LED_OFF, LED_ON, false);
} else if (engine.runTime() < 23_000.0) { // RUNNING LOW ON TIME
ledTopRed.setState(LED_OFF);
ledTopGreen.setState(LED_ON);
ledAnimateChaser(LED_ON, LED_ON, false);
} else { // 5 SECONDS LEFT!
if (engine.runTime() < 29_000.0) {
ledTopRed.setState(LED_ON);
ledTopGreen.setState(LED_ON);
} else {
ledTopRed.setState(LED_ON);
ledTopGreen.setState(LED_OFF);
}
ledAnimateProgress(LED_ON, LED_OFF, (engine.runTime() - 23_000.0) / 5_000.0);
}
} else {
if (engine.runTime() >= 90_000.0) { // LAUNCH DRONE and DO CHIN UP
ledTopRed.setState(LED_OFF);
ledTopGreen.setState(LED_ON);
ledSetRailRedLEDs(LED_OFF);
ledSetRailGreenLEDs(LED_ON);
} else if (engine.runTime() >= 80_000.0) { // GET READY
if (ledChaserIndex == railRedLEDs.size() - 1) {
ledTopRed.setState(LED_OFF);
ledTopGreen.setState(LED_OFF);
} else {
ledTopRed.setState(LED_ON);
ledTopGreen.setState(LED_ON);
}
ledAnimateChaser(LED_ON, LED_ON, false);
} else { // KEEP CALM and CARRY ON
ledTopRed.setState(LED_ON);
ledTopGreen.setState(LED_OFF);
ledAnimateChaser(LED_ON, LED_OFF, false);
}
}
}
public void ledSetRailGreenLEDs(boolean ledState) {
for (DigitalChannel led : railGreenLEDs) {
led.setState(ledState);
}
}
public void ledSetRailRedLEDs(boolean ledState) {
for (DigitalChannel led : railRedLEDs) {
led.setState(ledState);
}
}
public void ledAnimateChaser(boolean ledRed, boolean ledGreen, boolean invert) {
if (System.currentTimeMillis() - lastLedChaserTimeMS >= ledChaserIntervalMS) {
lastLedChaserTimeMS = System.currentTimeMillis();
} else {
return;
}
// Turn off current LED
if (invert) {
railRedLEDs.get(ledChaserIndex).setState(ledRed);
railGreenLEDs.get(ledChaserIndex).setState(ledGreen);
} else {
railRedLEDs.get(ledChaserIndex).setState(LED_OFF);
railGreenLEDs.get(ledChaserIndex).setState(LED_OFF);
}
// Cycle up/down
if (ledChaseUp) {
ledChaserIndex++;
} else {
ledChaserIndex--;
}
// Switch active LED
if (ledChaserIndex >= railRedLEDs.size()) {
ledChaseUp = !ledChaseUp;
ledChaserIndex = railRedLEDs.size() - 2;
} else if (ledChaserIndex < 0) {
ledChaseUp = !ledChaseUp;
ledChaserIndex = 1;
}
// Control active LED
if (!invert) {
railRedLEDs.get(ledChaserIndex).setState(ledRed);
railGreenLEDs.get(ledChaserIndex).setState(ledGreen);
} else {
railRedLEDs.get(ledChaserIndex).setState(LED_OFF);
railGreenLEDs.get(ledChaserIndex).setState(LED_OFF);
}
}
public void ledAnimateProgress(boolean progressLEDsRed, boolean progressLEDsGreen, double ratio) {
double i = 0.0;
for (DigitalChannel led : railRedLEDs) {
i += 1.0 / railRedLEDs.size();
if (ratio < i) {
led.setState(LED_OFF);
} else {
led.setState(progressLEDsRed);
}
}
i = 0.0;
for (DigitalChannel led : railGreenLEDs) {
i += 1.0 / railGreenLEDs.size();
if (ratio < i) {
led.setState(LED_OFF);
} else {
led.setState(progressLEDsGreen);
}
}
}
}

2
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/cyberarm_RedCrab.json
View File

File diff suppressed because one or more lines are too long

16
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/LEDControllerTask.java Normal file
View File

@@ -0,0 +1,16 @@
package dev.cyberarm.minibots.red_crab.engines;
import dev.cyberarm.engine.V2.CyberarmState;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
public class LEDControllerTask extends CyberarmState {
private final RedCrabMinibot robot;
public LEDControllerTask(RedCrabMinibot robot) {
this.robot = robot;
}
@Override
public void exec() {
robot.ledController();
}
}

1
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java
View File

@@ -14,6 +14,7 @@ public class RedCrabAutonomousBlueAudienceEngine extends RedCrabEngine {
public void setup() {
robot = new RedCrabMinibot(true);
addTask(new ClawArmTask(robot));
addTask(new LEDControllerTask(robot));
addTask(new LocalizerTask(robot));
setupFromConfig(

1
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java
View File

@@ -14,6 +14,7 @@ public class RedCrabAutonomousBlueBackstageEngine extends RedCrabEngine {
public void setup() {
robot = new RedCrabMinibot(true);
addTask(new ClawArmTask(robot));
addTask(new LEDControllerTask(robot));
addTask(new LocalizerTask(robot));
setupFromConfig(

1
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousDebuggingOnlyEngine.java
View File

@@ -14,6 +14,7 @@ public class RedCrabAutonomousDebuggingOnlyEngine extends RedCrabEngine {
public void setup() {
robot = new RedCrabMinibot(true);
addTask(new ClawArmTask(robot));
addTask(new LEDControllerTask(robot));
addTask(new LocalizerTask(robot));
setupFromConfig(

1
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java
View File

@@ -14,6 +14,7 @@ public class RedCrabAutonomousRedAudienceEngine extends RedCrabEngine {
public void setup() {
robot = new RedCrabMinibot(true);
addTask(new ClawArmTask(robot));
addTask(new LEDControllerTask(robot));
addTask(new LocalizerTask(robot));
setupFromConfig(

1
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java
View File

@@ -14,6 +14,7 @@ public class RedCrabAutonomousRedBackstageEngine extends RedCrabEngine {
public void setup() {
robot = new RedCrabMinibot(true);
addTask(new ClawArmTask(robot));
addTask(new LEDControllerTask(robot));
addTask(new LocalizerTask(robot));
setupFromConfig(

24
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabEngine.java
View File

@@ -14,30 +14,6 @@ public abstract class RedCrabEngine extends CyberarmEngine {
super.loop();
robot.standardTelemetry();
if (robot.autonomous) {
if (runTime() < 20_000.0) { // KEEP CALM and CARRY ON
robot.redLED.setState(robot.LED_OFF);
robot.greenLED.setState(robot.LED_ON);
} else if (runTime() < 25_000.0) { // RUNNING LOW ON TIME
robot.redLED.setState(robot.LED_ON);
robot.greenLED.setState(robot.LED_ON);
} else { // 5 SECONDS LEFT!
robot.redLED.setState(robot.LED_ON);
robot.greenLED.setState(robot.LED_OFF);
}
} else {
if (runTime() >= 90_000.0) { // LAUNCH DRONE and DO CHIN UP
robot.redLED.setState(robot.LED_OFF);
robot.greenLED.setState(robot.LED_ON);
} else if (runTime() >= 80_000.0) { // GET READY
robot.redLED.setState(robot.LED_ON);
robot.greenLED.setState(robot.LED_ON);
} else { // KEEP CALM and CARRY ON
robot.redLED.setState(robot.LED_ON);
robot.greenLED.setState(robot.LED_OFF);
}
}
}
@Override

1
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpDebuggingOnlyEngine.java
View File

@@ -18,6 +18,7 @@ public class RedCrabTeleOpDebuggingOnlyEngine extends RedCrabEngine {
robot = new RedCrabMinibot(false);
addTask(new LEDControllerTask(robot));
addTask(new LocalizerTask(robot));
addState(new CyberarmState() {

1
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java
View File

@@ -16,6 +16,7 @@ public class RedCrabTeleOpEngine extends RedCrabEngine {
robot = new RedCrabMinibot(false);
addTask(new ClawArmTask(robot));
addTask(new LEDControllerTask(robot));
addTask(new LocalizerTask(robot));
addState(new Pilot(robot));

3
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceBlue.java
View File

@@ -1,6 +1,7 @@
package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
@@ -15,7 +16,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "Burnsville audience blue", preselectTeleOp = "Competition V1 TeleOp")
@Disabled
public class CompetitionBurnsvilleAudienceBlue extends CyberarmEngine {
CompetitionRobotV1 robot;

2
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceRed.java
View File

@@ -1,6 +1,7 @@
package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
@@ -14,6 +15,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "Burnsville audience red", preselectTeleOp = "Competition V1 TeleOp")
@Disabled
public class CompetitionBurnsvilleAudienceRed extends CyberarmEngine {

14
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropBlue.java
View File

@@ -1,6 +1,7 @@
package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
@@ -15,6 +16,7 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "Burnsville BackDrop blue", preselectTeleOp = "Competition V1 TeleOp")
@Disabled
public class CompetitionBurnsvilleBackDropBlue extends CyberarmEngine {
@@ -41,17 +43,17 @@ public class CompetitionBurnsvilleBackDropBlue extends CyberarmEngine {
// addTask(new ClawArmControlTask(robot));
this.robot.setup();
addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-0"));
// addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-0"));
addState(new CameraVisionState(robot));
// addState(new CameraVisionState(robot));
addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-1"));
// addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-1"));
// drive to the left, center, or right spike mark
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "3-02-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "3-02-1"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "2-02-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "1-02-0"));
// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "3-02-1"));
// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "2-02-0"));
// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "1-02-0"));
addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-02-4"));

2
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropRed.java
View File

@@ -1,6 +1,7 @@
package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
@@ -14,6 +15,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "Burnsville BackDrop red", preselectTeleOp = "Competition V1 TeleOp")
@Disabled
public class CompetitionBurnsvilleBackDropRed extends CyberarmEngine {

106
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateBackDropBlue.java Normal file
View File

@@ -0,0 +1,106 @@
package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesTask;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.OdometryLocalizerTask;
import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "State BackDrop red", preselectTeleOp = "Competition V1 TeleOp")
public class StateBackDropBlue extends CyberarmEngine {
CompetitionRobotV1 robot;
@Override
public void init() {
super.init();
robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.clawArm.setTargetPosition(0);
robot.clawArm.setPower(0);
robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.imu.resetYaw();
robot.leftClaw.setPosition(0.25);
robot.rightClaw.setPosition(0.6);
}
@Override
public void setup() {
this.robot = new CompetitionRobotV1("State BackDrop red");
addTask(new DriveToCoordinatesTask(robot));
addTask(new OdometryLocalizerTask(robot));
this.robot.setup();
addState(new CameraVisionState(robot));
addState(new ClawArmState(robot,"State BackDrop red", "1-00-0"));
// drive to the left, center, or right spike mark
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-1"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-2"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-02-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-03-0"));
// bring arm to hover
addState(new ClawArmState(robot,"State BackDrop red", "3-00-0"));
//open claw
addState(new ClawFingerState(robot,"State BackDrop red", "4-00-0"));
addState(new ClawArmState(robot,"State BackDrop red", "5-00-0"));
// drive towards backboard
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-1"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-1"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-1"));
// pause
addState(new ClawArmState(robot,"State BackDrop red", "6-00-0"));
// drive into board
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-2"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-2"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-2"));
// pause
addState(new ClawArmState(robot,"State BackDrop red", "6-00-1"));
//open right close left
addState(new ClawFingerState(robot,"State BackDrop red", "7-00-0"));
// bring arm up
addState(new ClawArmState(robot,"State BackDrop red", "8-00-0"));
// drivw to park
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-01-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-02-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-03-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-03-1"));
addState(new ClawArmState(robot,"State BackDrop red", "9-00-0"));
// 65, on the left, 235 on the right
}
}

106
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateBackDropRed.java Normal file
View File

@@ -0,0 +1,106 @@
package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesTask;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.OdometryLocalizerTask;
import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "State BackDrop red", preselectTeleOp = "Competition V1 TeleOp")
public class StateBackDropRed extends CyberarmEngine {
CompetitionRobotV1 robot;
@Override
public void init() {
super.init();
robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.clawArm.setTargetPosition(0);
robot.clawArm.setPower(0);
robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.imu.resetYaw();
robot.leftClaw.setPosition(0.25);
robot.rightClaw.setPosition(0.6);
}
@Override
public void setup() {
this.robot = new CompetitionRobotV1("State BackDrop red");
addTask(new DriveToCoordinatesTask(robot));
addTask(new OdometryLocalizerTask(robot));
this.robot.setup();
addState(new CameraVisionState(robot));
addState(new ClawArmState(robot,"State BackDrop red", "1-00-0"));
// drive to the left, center, or right spike mark
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-1"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-2"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-02-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-03-0"));
// bring arm to hover
addState(new ClawArmState(robot,"State BackDrop red", "3-00-0"));
//open claw
addState(new ClawFingerState(robot,"State BackDrop red", "4-00-0"));
addState(new ClawArmState(robot,"State BackDrop red", "5-00-0"));
// drive towards backboard
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-1"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-1"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-1"));
// pause
addState(new ClawArmState(robot,"State BackDrop red", "6-00-0"));
// drive into board
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-2"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-2"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-2"));
// pause
addState(new ClawArmState(robot,"State BackDrop red", "6-00-1"));
//open right close left
addState(new ClawFingerState(robot,"State BackDrop red", "7-00-0"));
// bring arm up
addState(new ClawArmState(robot,"State BackDrop red", "8-00-0"));
// drivw to park
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-01-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-02-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-03-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-03-1"));
addState(new ClawArmState(robot,"State BackDrop red", "9-00-0"));
}
}

7
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/CameraVisionState.java
View File

@@ -54,7 +54,6 @@ public class CameraVisionState extends CyberarmState {
@Override
public void exec() {
robot.clawArmControl();
if (System.currentTimeMillis() - initTime > 4000){
robot.objectPos = pipeline.objectPos();
setHasFinished(true);
@@ -84,6 +83,7 @@ public class CameraVisionState extends CyberarmState {
Mat leftCrop;
Mat middleCrop;
Mat rightCrop;
Mat rotatedMat = new Mat();
double leftavgfin;
double middleavgfin;
double rightavgfin;
@@ -91,13 +91,14 @@ public class CameraVisionState extends CyberarmState {
Scalar rectColor = new Scalar(255.0, 0.0, 0.0);
public Mat processFrame(Mat input) {
Imgproc.cvtColor(input, HSV, Imgproc.COLOR_RGB2HSV);
Core.rotate(input, rotatedMat,Core.ROTATE_180);
Imgproc.cvtColor(rotatedMat, HSV, Imgproc.COLOR_RGB2HSV);
Rect leftRect = new Rect(1, 1, 212, 359);
Rect rightRect = new Rect(213, 1, 212, 359);
Rect middleRect = new Rect(426, 1, 212, 359);
input.copyTo(output);
rotatedMat.copyTo(output);
Imgproc.rectangle(output, leftRect, rectColor, 2);
Imgproc.rectangle(output, rightRect, rectColor, 2);
Imgproc.rectangle(output, middleRect, rectColor, 2);

16
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java
View File

@@ -15,12 +15,12 @@ public class DriveToCoordinatesState extends CyberarmState {
public double xTarget;
public double yTarget;
public double hTarget;
public boolean posAchieved = false;
public boolean armDrive;
public int objectPos;
public boolean posSpecific;
public double maxXPower;
public double maxYPower;
public double maxHPower;
private String actionName;
public DriveToCoordinatesState(CompetitionRobotV1 robot, String groupName, String actionName) {
@@ -32,6 +32,7 @@ public class DriveToCoordinatesState extends CyberarmState {
this.hTarget = robot.configuration.variable(groupName, actionName, "hTarget").value();
this.maxXPower = robot.configuration.variable(groupName, actionName, "maxXPower").value();
this.maxYPower = robot.configuration.variable(groupName, actionName, "maxYPower").value();
this.maxHPower = robot.configuration.variable(groupName, actionName, "maxHPower").value();
this.armDrive = robot.configuration.variable(groupName, actionName, "armDrive").value();
this.objectPos = robot.configuration.variable(groupName, actionName, "objectPos").value();
this.posSpecific = robot.configuration.variable(groupName, actionName, "posSpecific").value();
@@ -40,23 +41,22 @@ public class DriveToCoordinatesState extends CyberarmState {
@Override
public void start() {
super.start();
if (posSpecific) {
if (posSpecific && objectPos == robot.objectPos) {
robot.hTarget = hTarget;
robot.yTarget = yTarget;
robot.xTarget = xTarget;
robot.yMaxPower = maxYPower;
robot.xMaxPower = maxXPower;
robot.hMaxPower = maxHPower;
} else {
setHasFinished(true);
}
Log.d("TTT?", "" + actionName + " CURRENT POSITION: x: " + robot.positionX + " Y: " + robot.positionY + "h: " + robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
Log.d("TTT?", "" + actionName + " TARGET POSITION: x: " + robot.xTarget + " Y: " + robot.yTarget + "h: " + robot.hTarget);
}
@Override
public void exec() {
if (posSpecific) {
if (objectPos != robot.objectPos) {
// enter the ending loop
@@ -68,7 +68,8 @@ public class DriveToCoordinatesState extends CyberarmState {
}
if (Math.abs(robot.positionX - robot.xTarget) < 5
&& Math.abs(robot.positionY - robot.yTarget) < 5) {
&& Math.abs(robot.positionY - robot.yTarget) < 5
&& Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)) - Math.abs(Math.toDegrees(robot.hTarget)) < 5) {
setHasFinished(true);
}
}
@@ -78,7 +79,8 @@ public class DriveToCoordinatesState extends CyberarmState {
}
if (Math.abs(robot.positionX - robot.xTarget) < 5
&& Math.abs(robot.positionY - robot.yTarget) < 5) {
&& Math.abs(robot.positionY - robot.yTarget) < 5
&& Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)) - Math.abs(Math.toDegrees(robot.hTarget)) < 5) {
setHasFinished(true);
}
}

1
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesTask.java
View File

@@ -18,6 +18,7 @@ public class DriveToCoordinatesTask extends CyberarmState {
public void exec() {
robot.XDrivePowerModifier();
robot.YDrivePowerModifier();
robot.HDrivePowerModifier();
robot.DriveToCoordinates();
}
}

81
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java
View File

@@ -51,10 +51,8 @@ public class CompetitionRobotV1 extends Robot {
// ----------------------------------------------------------------------------------------------------------------- odometry variables:
public static double Hp = 0.8, Hi = 0, Hd = 0;
public static double Xp = -0.03, Xi = 0, Xd = 0;
public static double xvp = -0.03, xvi = 0, xvd = 0;
public static double Yp = 0.03, Yi = 0, Yd = 0;
public static double yvp = 0.03, yvi = 0, yvd = 0;
public static double Xp = 0.03, Xi = 0, Xd = 0;
public static double Yp = -0.03, Yi = 0, Yd = 0;
public double Dnl1;
public double Dnr2;
@@ -75,8 +73,8 @@ public class CompetitionRobotV1 extends Robot {
public int oldRightPosition = 0;
public int oldLeftPosition = 0;
public int oldAuxPosition = 0;
public final static double L = 22.5; // distance between left and right encoder in cm
final static double B = 15; // distance between the midpoint of the left and right encoder from the auxillary encoder in cm
public final static double L = 20.9; // distance between left and right encoder in cm
final static double B = 12.6; // distance between the midpoint of the left and right encoder from the auxillary encoder in cm
public final static double R = 3; // wheel radius in cm
final static double N = 8192; // encoder ticks per revolution (REV encoder)
@@ -105,27 +103,29 @@ public class CompetitionRobotV1 extends Robot {
public double yMaxPower = 1;
public double xMaxPower = 1;
public double hMaxPower = 1;
public double pidX;
public double pidY;
public double pidH;
public double rawPidX;
public double rawPidY;
public double rawPidH;
public double xVelocity;
public double yVelocity;
public double deltaTime;
public double deltaTime = 0;
//-------------------------------------------------------------------------------------------------------------- arm sequence variables:
PIDController pidController;
public double power;
public String armPos;
public String armPos = "hover";
public long armTime;
public int target;
public double p = 0.007, i = 0, d = 0.0001, f = 0;
public double shoulderCollect = 0.86;
public double shoulderDeposit = 0.86;
public double shoulderDeposit = 1;
public double shoulderPassive = 1;
public double elbowCollect = 0.02;
public double elbowDeposit = 0;
@@ -286,7 +286,7 @@ public class CompetitionRobotV1 extends Robot {
* A Controller taking error of the heading position and converting to a power in the direction of a target.
* @param reference reference is the target position
* @param current current is the measured sensor value.
* @return A power to the target the position
* @return A power to the target position
*
*/
public double HeadingPIDControl(double reference, double current){
@@ -299,27 +299,6 @@ public class CompetitionRobotV1 extends Robot {
double output = (error * Hp) + (derivative * Hd) + (headingIntegralSum * Hi);
return output;
}
public double XVeloPIDControl ( double reference, double current){
double error = (reference - current);
xVeloIntegralSum += error * xVeloTimer.seconds();
double derivative = (error - xVeloLastError) / xVeloTimer.seconds();
xTimer.reset();
double output = (error * xvp) + (derivative * xvd) + (xIntegralSum * xvi);
return output;
}
public double YVeloPIDControl ( double reference, double current){
double error = (reference - current);
yVeloIntegralSum += error * yVeloTimer.seconds();
double derivative = (error - yVeloLastError) / xTimer.seconds();
xTimer.reset();
double output = (error * yvp) + (derivative * yvd) + (yVeloIntegralSum * yvi);
return output;
}
public double XPIDControl ( double reference, double current){
double error = (reference - current);
@@ -372,17 +351,27 @@ public class CompetitionRobotV1 extends Robot {
pidX = rawPidX;
}
}
public void HDrivePowerModifier () {
rawPidH = HeadingPIDControl(Math.toRadians(hTarget), imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
if (Math.abs(rawPidH) > hMaxPower) {
if (rawPidH < 0) {
pidH = -hMaxPower;
} else {
pidH = hMaxPower;
}
} else {
pidH = rawPidH;
}
}
public void DriveToCoordinates () {
// determine the powers needed for each direction
// this uses PID to adjust needed Power for robot to move to target
XVeloPIDControl(targetVelocityX, xVelocity);
YVeloPIDControl(targetVelocityY, yVelocity);
double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
double rx = HeadingPIDControl(Math.toRadians(hTarget), heading);
double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(rx), 1);
double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(pidH), 1);
// field oriented math, (rotating the global field to the relative field)
double rotY = pidY * Math.cos(heading) - pidX * Math.sin(heading);
@@ -390,10 +379,10 @@ public class CompetitionRobotV1 extends Robot {
// finding approximate power for each wheel.
frontLeftPower = (rotY + rotX + rx) / denominator;
backLeftPower = (rotY - rotX + rx) / denominator;
frontRightPower = (rotY - rotX - rx) / denominator;
backRightPower = (rotY + rotX - rx) / denominator;
frontLeftPower = (rotY + rotX + pidH) / denominator;
backLeftPower = (rotY - rotX + pidH) / denominator;
frontRightPower = (rotY - rotX - pidH) / denominator;
backRightPower = (rotY + rotX - pidH) / denominator;
// apply my powers
frontLeft.setPower(frontLeftPower);
@@ -416,7 +405,7 @@ public class CompetitionRobotV1 extends Robot {
if (Objects.equals(armPos, "deposit")) {
shoulder.setPosition(shoulderDeposit);
elbow.setPosition(elbowDeposit);
target = 400;
target = 300;
}
@@ -430,19 +419,13 @@ public class CompetitionRobotV1 extends Robot {
}
if (armPos.equals("search")) {
shoulder.setPosition(0.48);
if (armTime > 400){
shoulder.setPosition(0.55);
if (armTime > 450){
target = 570;
}
}
// pidController.setPID(p, i, d);
// int armPos = clawArm.getCurrentPosition();
// double pid = pidController.calculate(armPos, target);
//
// power = pid;
clawArm.setTargetPosition(target);
clawArm.setPower(0.4);

26
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java
View File

@@ -28,11 +28,14 @@ public class CompetitionTeleOpState extends CyberarmState {
public static double holdPos = 0.55 ;
public double maxVelocityX;
public double maxVelocityY;
// ------------------------------------------------------------------------------------------------------------- Heading lock variables:
public double integralSum = 0;
private double targetHeading;
public double collectLock = Math.toRadians(90);
public double backDropLock = Math.toRadians(-90);
public double collectLock = Math.toRadians(-90);
public double backDropLock = Math.toRadians(90);
public double boost;
public double armPower;
private double currentHeading;
@@ -68,8 +71,8 @@ public class CompetitionTeleOpState extends CyberarmState {
// ---------------------------------------------------------------------------------------------------------------Arm control Variables:
public String armPos = "collect";
// chin up servo
public static double chinUpServoUp = 0.58;
public static double chinUpServoDown = 1;
public static double chinUpServoUp = 0.7;
public static double chinUpServoDown = 0;
public long lastExecutedTime;
@@ -110,7 +113,6 @@ public class CompetitionTeleOpState extends CyberarmState {
}
}
public void DriveTrainTeleOp () {
boolean lbs1 = engine.gamepad1.left_stick_button;
@@ -337,10 +339,17 @@ public class CompetitionTeleOpState extends CyberarmState {
robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
lastExecutedTime = System.currentTimeMillis();
}
@Override
public void exec() {
public void exec() { //------------------------------------------------------------------------------------------------------ EXEC Start
if (robot.xVelocity > maxVelocityX){
maxVelocityX = robot.xVelocity;
}
if (robot.yVelocity > maxVelocityY){
maxVelocityY = robot.yVelocity;
}
robot.OdometryLocalizer();
if (engine.gamepad2.start && engine.gamepad2.x){
@@ -409,10 +418,15 @@ public class CompetitionTeleOpState extends CyberarmState {
ClawControlTeleOp();
robot.velocityChecker();
}
@Override
public void telemetry () {
engine.telemetry.addData("x velocity max", maxVelocityX);
engine.telemetry.addData("y velocity max", maxVelocityY);
engine.telemetry.addData("Dnl1", robot.Dnl1);
engine.telemetry.addData("Dnr2", robot.Dnr2);
engine.telemetry.addData("x pos", robot.positionX);