TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-16 01:02:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

Compare commits

base: TimeCrafters:71374a57eaab983d9c84d10a88a409b7a0cda91d
Branches Tags
TimeCrafters:master
...
compare: TimeCrafters:master
Branches Tags
TimeCrafters:master

12 Commits
71374a57ea ... master

Author SHA1 Message Date
SpencerPiha
ed993eff76 Merge remote-tracking branch 'origin/master' 2024-02-16 15:16:23 -06:00
SpencerPiha
3b868f10a0 added a blue and red audience side autonomous (it works and has been tested) 2024-02-16 15:16:11 -06:00
Cyberarm
704708f907 RedCrab: Make LED rail show launch confirmation progress in teleop, add null check for localizer loadConstants function 2024-02-13 20:59:36 -06:00
NerdyBirdy460
810914dd77 Merge remote-tracking branch 'origin/master' 2024-02-13 20:50:54 -06:00
NerdyBirdy460
3424549d61 Back to robot-centric :( 2024-02-13 20:50:45 -06:00
SpencerPiha
255f75e3d4 auto work right side done 2024-02-13 20:31:48 -06:00
SpencerPiha
c12c813a14 inverted logic fixed for the finish in drive to coordinates state 2024-02-11 18:26:08 -06:00
SpencerPiha
b57b4c54f1 Disabled all of my old burnsville autos so they don't show. I also implemented a max turning power. 2024-02-11 18:23:52 -06:00
SpencerPiha
da958f567e Merge remote-tracking branch 'origin/master' 2024-02-10 14:59:16 -06:00
SpencerPiha
b8814b1084 tuning auto today 2024-02-10 14:59:07 -06:00
Cyberarm
45389badd0 RedCrab: Localizer constants are now re/loaded from config 2024-02-10 13:49:23 -06:00
Cyberarm
3285f540bb RedCrab: Animated LEDs 2024-02-10 12:55:39 -06:00
27 changed files with 607 additions and 103 deletions
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() {

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

@@ -92,7 +92,12 @@ public class RedCrabMinibot {
public final DigitalChannel ledTopGreen, ledTopRed, ledRail0Green, ledRail0Red, ledRail1Green, ledRail1Red, ledRail2Green, ledRail2Red,
ledRail3Green, ledRail3Red;
public final ArrayList<DigitalChannel> railGreenLEDs = new ArrayList<>(), railRedLEDs = new ArrayList<>();
public final boolean LED_OFF = true, LED_ON = false;
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;
@@ -100,6 +105,9 @@ public class RedCrabMinibot {
public TimeCraftersConfiguration config;
private final PIDFController clawArmPIDFController;
public final String webcamName = "Webcam 1";
public boolean droneLaunchRequested = false;
public double droneLastLaunchRequestStartMS = 0;
public boolean droneLaunchAuthorized = false;
private long lastClawArmOverCurrentAnnounced = 0;
private boolean clawArmOverCurrent = false;
@@ -299,6 +307,9 @@ public class RedCrabMinibot {
led.setState(LED_ON);
}
ledChaserIntervalMS = 250;
lastLedChaserTimeMS = System.currentTimeMillis();
// Bulk read from hubs
Utilities.hubsBulkReadMode(engine.hardwareMap, LynxModule.BulkCachingMode.MANUAL);
@@ -378,6 +389,9 @@ 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();
if (RedCrabMinibot.localizer != null)
RedCrabMinibot.localizer.loadConfigConstants();
}
public void resetDeadWheels() {
@@ -625,22 +639,42 @@ public class RedCrabMinibot {
ledTopRed.setState(LED_OFF);
ledTopGreen.setState(LED_ON);
ledSetRailRedLEDs(LED_OFF);
ledSetRailGreenLEDs(LED_ON);
} else if (engine.runTime() < 25_000.0) { // RUNNING LOW ON TIME
ledTopRed.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);
ledSetRailRedLEDs(LED_ON);
ledSetRailGreenLEDs(LED_ON);
ledAnimateChaser(LED_ON, LED_ON, false);
} else { // 5 SECONDS LEFT!
ledTopRed.setState(LED_ON);
ledTopGreen.setState(LED_OFF);
if (engine.runTime() < 29_000.0) {
ledTopRed.setState(LED_ON);
ledTopGreen.setState(LED_ON);
} else {
ledTopRed.setState(LED_ON);
ledTopGreen.setState(LED_OFF);
}
ledSetRailRedLEDs(LED_ON);
ledSetRailGreenLEDs(LED_OFF);
ledAnimateProgress(LED_ON, LED_OFF, (engine.runTime() - 23_000.0) / 5_000.0);
}
} else {
// Show progress of drone launch authorization
if (droneLaunchRequested) {
if (droneLaunchAuthorized) {
ledTopRed.setState(LED_OFF);
ledTopGreen.setState(LED_ON);
ledSetRailRedLEDs(LED_ON);
ledSetRailRedLEDs(LED_ON);
} else {
ledTopRed.setState(LED_ON);
ledTopGreen.setState(LED_ON);
ledAnimateProgress(LED_ON, LED_ON, (engine.runTime() - droneLastLaunchRequestStartMS) / RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS);
}
return;
}
if (engine.runTime() >= 90_000.0) { // LAUNCH DRONE and DO CHIN UP
ledTopRed.setState(LED_OFF);
ledTopGreen.setState(LED_ON);
@@ -648,17 +682,20 @@ public class RedCrabMinibot {
ledSetRailRedLEDs(LED_OFF);
ledSetRailGreenLEDs(LED_ON);
} else if (engine.runTime() >= 80_000.0) { // GET READY
ledTopRed.setState(LED_ON);
ledTopGreen.setState(LED_ON);
if (ledChaserIndex == railRedLEDs.size() - 1) {
ledTopRed.setState(LED_OFF);
ledTopGreen.setState(LED_OFF);
} else {
ledTopRed.setState(LED_ON);
ledTopGreen.setState(LED_ON);
}
ledSetRailRedLEDs(LED_ON);
ledSetRailGreenLEDs(LED_ON);
ledAnimateChaser(LED_ON, LED_ON, false);
} else { // KEEP CALM and CARRY ON
ledTopRed.setState(LED_ON);
ledTopGreen.setState(LED_OFF);
ledSetRailRedLEDs(LED_ON);
ledSetRailGreenLEDs(LED_OFF);
ledAnimateChaser(LED_ON, LED_OFF, false);
}
}
}
@@ -674,4 +711,70 @@ public class RedCrabMinibot {
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(

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

@@ -14,8 +14,6 @@ public abstract class RedCrabEngine extends CyberarmEngine {
super.loop();
robot.standardTelemetry();
robot.ledController();
}
@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));

23
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java
View File

@@ -17,9 +17,6 @@ public class Pilot extends CyberarmState {
private boolean rightClawOpen = false;
private int clawArmPosition = RedCrabMinibot.ClawArm_INITIAL;
private boolean hookArmUp = false;
private boolean droneLaunchAuthorized = false;
private boolean droneLaunchRequested = false;
private double droneLastLaunchRequestStartMS = 0;
private double odometryResetRequestStartMS = 0;
private boolean odometryResetRequested = false;
private boolean odometryResetRequestLeftStick = false;
@@ -42,7 +39,7 @@ public class Pilot extends CyberarmState {
clawArmAndWristController();
clawController();
droneLatchController();
hookArmController(); // disabled for swrist debug
hookArmController();
winchController();
odometryDebugController();
@@ -54,8 +51,8 @@ public class Pilot extends CyberarmState {
if (gamepad == engine.gamepad1) {
switch (button) {
case "y":
droneLaunchRequested = true;
droneLastLaunchRequestStartMS = runTime();
robot.droneLaunchRequested = true;
robot.droneLastLaunchRequestStartMS = runTime();
break;
case "x":
hookArmUp = true;
@@ -117,8 +114,8 @@ public class Pilot extends CyberarmState {
if (gamepad == engine.gamepad1) {
switch (button) {
case "y":
droneLaunchRequested = false;
droneLastLaunchRequestStartMS = runTime();
robot.droneLaunchRequested = false;
robot.droneLastLaunchRequestStartMS = runTime();
break;
case "left_stick_button":
odometryResetRequestLeftStick = false;
@@ -234,16 +231,16 @@ public class Pilot extends CyberarmState {
}
private void droneLatchController() {
if (droneLaunchRequested && runTime() - droneLastLaunchRequestStartMS >= RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS)
droneLaunchAuthorized = true;
if (robot.droneLaunchRequested && runTime() - robot.droneLastLaunchRequestStartMS >= RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS)
robot.droneLaunchAuthorized = true;
if (droneLaunchAuthorized) {
if (robot.droneLaunchAuthorized) {
robot.droneLatch.setPosition(RedCrabMinibot.DRONE_LATCH_LAUNCH_POSITION);
}
// Auto reset drone latch after DRONE_LAUNCH_CONFIRMATION_TIME_MS + 1 second
if (!droneLaunchRequested && runTime() - droneLastLaunchRequestStartMS >= RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS + 1_000) {
droneLaunchAuthorized = false;
if (!robot.droneLaunchRequested && runTime() - robot.droneLastLaunchRequestStartMS >= RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS + 1_000) {
robot.droneLaunchAuthorized = false;
robot.droneLatch.setPosition(RedCrabMinibot.DRONE_LATCH_INITIAL_POSITION);
}

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 {

2
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 {

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 {

73
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateAudienceBlue.java Normal file
View File

@@ -0,0 +1,73 @@
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 Audience blue", preselectTeleOp = "Competition V1 TeleOp")
public class StateAudienceBlue 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", "9-00-0"));
}
}

70
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateAudienceRed.java Normal file
View File

@@ -0,0 +1,70 @@
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 Audience red", preselectTeleOp = "Competition V1 TeleOp")
public class StateAudienceRed 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 blue");
addTask(new DriveToCoordinatesTask(robot));
addTask(new OdometryLocalizerTask(robot));
this.robot.setup();
addState(new CameraVisionState(robot));
addState(new ClawArmState(robot,"State BackDrop blue", "1-00-0"));
// drive to the left, center, or right spike mark
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-1"));
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-2"));
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-02-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-01-0"));
// bring arm to hover
addState(new ClawArmState(robot,"State BackDrop blue", "3-00-0"));
//open claw
addState(new ClawFingerState(robot,"State BackDrop blue", "4-00-0"));
addState(new ClawArmState(robot,"State BackDrop blue", "9-00-0"));
}
}

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 blue", 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 blue");
addTask(new DriveToCoordinatesTask(robot));
addTask(new OdometryLocalizerTask(robot));
this.robot.setup();
addState(new CameraVisionState(robot));
addState(new ClawArmState(robot,"State BackDrop blue", "1-00-0"));
// drive to the left, center, or right spike mark
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-1"));
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-2"));
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-02-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-01-0"));
// bring arm to hover
addState(new ClawArmState(robot,"State BackDrop blue", "3-00-0"));
//open claw
addState(new ClawFingerState(robot,"State BackDrop blue", "4-00-0"));
addState(new ClawArmState(robot,"State BackDrop blue", "5-00-0"));
// drive towards backboard
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-01-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-01-1"));
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-02-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-02-1"));
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-03-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-03-1"));
// pause
addState(new ClawArmState(robot,"State BackDrop blue", "6-00-0"));
// drive into board
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-01-2"));
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-02-2"));
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-03-2"));
// pause
addState(new ClawArmState(robot,"State BackDrop blue", "6-00-1"));
//open right close left
addState(new ClawFingerState(robot,"State BackDrop blue", "7-00-0"));
// bring arm up
addState(new ClawArmState(robot,"State BackDrop blue", "8-00-0"));
// drivw to park
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "9-01-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "9-02-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "9-03-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "9-03-1"));
addState(new ClawArmState(robot,"State BackDrop blue", "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);

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

@@ -12,18 +12,15 @@ import dev.cyberarm.engine.V2.CyberarmState;
public class DriveToCoordinatesState extends CyberarmState {
CompetitionRobotV1 robot;
public static double xTarget;
public static double yTarget;
public static double hTarget;
public static double maxVelocityX;
public static double maxVelocityY;
public double maxVelocityH;
public boolean posAchieved = false;
public double xTarget;
public double yTarget;
public double hTarget;
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) {
@@ -35,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();
@@ -43,33 +41,26 @@ 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 (robot.xVelocity > maxVelocityX){
maxVelocityX = robot.xVelocity;
}
if (robot.yVelocity > maxVelocityY){
maxVelocityY = robot.yVelocity;
}
if (posSpecific) {
if (objectPos != robot.objectPos) {
// enter the ending loop
// setHasFinished(true);
setHasFinished(true);
} else {
if (armDrive) {
@@ -77,8 +68,9 @@ public class DriveToCoordinatesState extends CyberarmState {
}
if (Math.abs(robot.positionX - robot.xTarget) < 5
&& Math.abs(robot.positionY - robot.yTarget) < 5) {
// setHasFinished(true);
&& Math.abs(robot.positionY - robot.yTarget) < 5
&& Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)) - Math.abs(Math.toDegrees(robot.hTarget)) < 5) {
setHasFinished(true);
}
}
} else {
@@ -87,16 +79,15 @@ public class DriveToCoordinatesState extends CyberarmState {
}
if (Math.abs(robot.positionX - robot.xTarget) < 5
&& Math.abs(robot.positionY - robot.yTarget) < 5) {
// setHasFinished(true);
&& Math.abs(robot.positionY - robot.yTarget) < 5
&& Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)) - Math.abs(Math.toDegrees(robot.hTarget)) < 5) {
setHasFinished(true);
}
}
}
@Override
public void telemetry() {
engine.telemetry.addData("x velocity max", maxVelocityX);
engine.telemetry.addData("y velocity max", maxVelocityY);
engine.telemetry.addData("x pos", robot.positionX);
engine.telemetry.addData("y pos", robot.positionY);
engine.telemetry.addData("h pos odo", Math.toDegrees(robot.positionH));

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();
}
}

54
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, 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;
@@ -76,7 +74,7 @@ public class CompetitionRobotV1 extends Robot {
public int oldLeftPosition = 0;
public int oldAuxPosition = 0;
public final static double L = 20.9; // 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
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,11 +103,13 @@ 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 = 0;
@@ -118,14 +118,14 @@ public class CompetitionRobotV1 extends Robot {
//-------------------------------------------------------------------------------------------------------------- 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){
@@ -351,15 +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
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);
@@ -367,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);
@@ -393,7 +405,7 @@ public class CompetitionRobotV1 extends Robot {
if (Objects.equals(armPos, "deposit")) {
shoulder.setPosition(shoulderDeposit);
elbow.setPosition(elbowDeposit);
target = 400;
target = 300;
}
@@ -407,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);

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

@@ -24,8 +24,8 @@ public class CompetitionTeleOpState extends CyberarmState {
public int target = 0;
// ---------------------------------------------------------------------------------------------------------------------shoot variables:
public static double releasePos = 0.95;
public static double holdPos = 0.55 ;
public static double releasePos = 0.45;
public static double holdPos = 0.95 ;
public double maxVelocityX;
@@ -34,8 +34,8 @@ public class CompetitionTeleOpState extends CyberarmState {
// ------------------------------------------------------------------------------------------------------------- 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;
@@ -71,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.55;
public static double chinUpServoDown = 0;
public long lastExecutedTime;
@@ -299,9 +299,8 @@ public class CompetitionTeleOpState extends CyberarmState {
}
if (Objects.equals(armPos, "lift up")) {
robot.shoulder.setPosition(robot.shoulderCollect);
robot.elbow.setPosition(robot.elbowDeposit);
target = 120;
robot.shoulder.setPosition(0.2);
target = 850;
robot.chinUpServo.setPosition(chinUpServoUp);
}
@@ -311,9 +310,8 @@ public class CompetitionTeleOpState extends CyberarmState {
robot.chinUpServo.setPosition(chinUpServoDown);
} else {
robot.lift.setPower(0);
robot.chinUpServo.setPosition(chinUpServoDown);
robot.shoulder.setPosition(robot.shoulderCollect);
target = 120;
robot.shoulder.setPosition(0.2);
target = 850;
}
}

12
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java
View File

@@ -85,7 +85,7 @@ public class SodiPizzaTeleOPState extends CyberarmState {
// lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
gp1checker = new GamepadChecker(engine, engine.gamepad1);
gp2checker = new GamepadChecker(engine, engine.gamepad1);
gp2checker = new GamepadChecker(engine, engine.gamepad2);
lastMoveTime = System.currentTimeMillis();
lastDistRead = System.currentTimeMillis();
@@ -138,6 +138,12 @@ public class SodiPizzaTeleOPState extends CyberarmState {
robot.imu.resetYaw();
}
if (Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)) > 0.000001 &&
Math.abs(robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES)) > 0.000001 &&
Math.abs(robot.imu.getRobotYawPitchRollAngles().getRoll(AngleUnit.DEGREES)) > 0.000001) {
robot.imu.resetYaw();
}
if (engine.gamepad1.left_stick_button) {
if (System.currentTimeMillis() - lastMoveTime >= 200) {
robot.launcher.setPosition(0.98);
@@ -163,12 +169,12 @@ public class SodiPizzaTeleOPState extends CyberarmState {
}
if (engine.gamepad1.dpad_up) {
if (System.currentTimeMillis() - lastMoveTime >= 200) {
if (System.currentTimeMillis() - lastMoveTime >= 150) {
robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
lastMoveTime = System.currentTimeMillis();
}
} else if (engine.gamepad1.dpad_down) {
if (System.currentTimeMillis() - lastMoveTime >= 200) {
if (System.currentTimeMillis() - lastMoveTime >= 150) {
robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
lastMoveTime = System.currentTimeMillis();
}