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

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
SpencerPiha
2024-01-10 20:05:13 -06:00
parent 25c40a7f28 adf856bb24
commit fc0fd6c77b
17 changed files with 150 additions and 126 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmEngine.java
View File

@@ -514,11 +514,15 @@ public abstract class CyberarmEngine extends OpMode {
RuntimeException exception = new RuntimeException(cause.getMessage(), cause.getCause());
exception.setStackTrace(cause.getStackTrace());
throw(exception);
} else {
e.printStackTrace();
RuntimeException exception = new RuntimeException(e.getMessage(), e.getCause());
exception.setStackTrace(e.getStackTrace());
throw(exception);
}
}
}

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

@@ -1,9 +1,6 @@
package dev.cyberarm.minibots.red_crab;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.arcrobotics.ftclib.hardware.motors.Motor;
import com.arcrobotics.ftclib.hardware.motors.MotorEx;
import com.arcrobotics.ftclib.hardware.motors.MotorGroup;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotor;
@@ -12,7 +9,6 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.Range;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
@@ -46,6 +42,7 @@ public class RedCrabMinibot {
public static double CLAW_ARM_MAX_SPEED = 0.5;
public static double CLAW_ARM_MAX_VELOCITY_DEGREES = 10;
private static double CLAW_ARM_MOTOR_MAX_CURRENT_MILLIAMPS = 1588.0;
private static long CLAW_ARM_WARN_OVERCURRENT_AFTER_MS = 5000;
public static double CLAW_ARM_kP = 0.0;
public static double CLAW_ARM_kI = 0.0;
public static double CLAW_ARM_kD = 0.0;
@@ -92,6 +89,8 @@ public class RedCrabMinibot {
private final PIDFController clawArmPIDFController;
public final String webcamName = "Webcam 1";
private long lastClawArmOverCurrentAnnounced = 0;
private boolean clawArmOverCurrent = false;
public enum Path {
LEFT,
CENTER,
@@ -195,7 +194,9 @@ public class RedCrabMinibot {
/// --- Claw Arm Motor
/// --- --- (SOFT) RESET MOTOR ENCODER
clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
// ONLY RESET ENCODER IN AUTONOMOUS
if (autonomous)
clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
/// --- --- DIRECTION
clawArm.setDirection(DcMotorSimple.Direction.FORWARD);
/// --- --- BRAKING
@@ -258,6 +259,7 @@ public class RedCrabMinibot {
RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO = config.variable("Robot", "ClawArm_Tuning", "gear_ratio").value();
RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION = config.variable("Robot", "ClawArm_Tuning", "motor_ticks").value();
RedCrabMinibot.CLAW_ARM_MOTOR_MAX_CURRENT_MILLIAMPS = config.variable("Robot", "ClawArm_Tuning", "max_current_milliamps").value();
RedCrabMinibot.CLAW_ARM_WARN_OVERCURRENT_AFTER_MS = config.variable("Robot", "ClawArm_Tuning", "warn_overcurrent_after_ms").value();
/// WINCH
@@ -412,6 +414,18 @@ public class RedCrabMinibot {
public void controlClawArm() {
Action action = config.action("Robot", "ClawArm_Tuning");
long milliseconds = System.currentTimeMillis();
if (clawArm.isOverCurrent())
{
if (milliseconds - lastClawArmOverCurrentAnnounced >= CLAW_ARM_WARN_OVERCURRENT_AFTER_MS) {
lastClawArmOverCurrentAnnounced = System.currentTimeMillis();
engine.telemetry.speak("WARNING. ARM. OVER. CURRENT.");
}
} else {
lastClawArmOverCurrentAnnounced = milliseconds;
}
double ticksInDegree = Utilities.motorAngleToTicks(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, 1);
for (Variable v : action.getVariables()) {
@@ -450,6 +464,18 @@ public class RedCrabMinibot {
double velocity = Utilities.motorAngleToTicks(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, CLAW_ARM_MAX_VELOCITY_DEGREES);
double currentAngle = Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, clawArm.getCurrentPosition());
double targetAngle = Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, clawArm.getTargetPosition());
double angleDiff = Math.abs(Utilities.angleDiff(currentAngle, targetAngle));
// Turn off motor if it is stowed or all the way down
if (targetAngle <= CLAW_ARM_STOW_ANGLE + 5.0 && angleDiff <= 5.0) {
velocity = 0.0;
} else if (targetAngle >= CLAW_ARM_COLLECT_ANGLE - 5.0 && angleDiff <= 5.0)
{
velocity = 0.0;
}
clawArm.setVelocity(velocity);
}

9
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/TeamPropVisionProcessor.java
View File

@@ -158,6 +158,15 @@ public class TeamPropVisionProcessor implements VisionProcessor {
return saturationRight;
}
public double getHighestSaturation() {
if (getSaturationLeft() > getSaturationCenter() && getSaturationLeft() > getSaturationRight())
return getSaturationLeft();
if (getSaturationCenter() > getSaturationLeft() && getSaturationCenter() > getSaturationRight())
return getSaturationCenter();
return getSaturationRight();
}
private android.graphics.Rect makeDrawableRect(Rect rect, float scaleBmpPxToCanvasPx) {
int left = Math.round(rect.x * scaleBmpPxToCanvasPx);
int top = Math.round(rect.y * scaleBmpPxToCanvasPx);

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

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

@@ -12,9 +12,7 @@ import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
public class RedCrabAutonomousBlueAudienceEngine extends RedCrabAutonomousEngine {
@Override
public void setup() {
RedCrabMinibot robot = new RedCrabMinibot(true);
blackboardSet("clawArmPower", 0.0);
robot = new RedCrabMinibot(true);
addTask(new ClawArmTask(robot));
setupFromConfig(

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

@@ -12,9 +12,7 @@ import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
public class RedCrabAutonomousBlueBackstageEngine extends RedCrabAutonomousEngine {
@Override
public void setup() {
RedCrabMinibot robot = new RedCrabMinibot(true);
blackboardSet("clawArmPower", 0.0);
robot = new RedCrabMinibot(true);
addTask(new ClawArmTask(robot));
setupFromConfig(

6
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousEngine.java
View File

@@ -2,12 +2,18 @@ package dev.cyberarm.minibots.red_crab.engines;
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.engine.V2.Utilities;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
public abstract class RedCrabAutonomousEngine extends CyberarmEngine {
protected RedCrabMinibot robot;
@Override
public void loop() {
Utilities.hubsClearBulkReadCache(hardwareMap);
super.loop();
if (robot != null)
robot.standardTelemetry();
}
}

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

@@ -12,9 +12,7 @@ import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
public class RedCrabAutonomousRedAudienceEngine extends RedCrabAutonomousEngine {
@Override
public void setup() {
RedCrabMinibot robot = new RedCrabMinibot(true);
blackboardSet("clawArmPower", 0.0);
robot = new RedCrabMinibot(true);
addTask(new ClawArmTask(robot));
setupFromConfig(

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

@@ -12,9 +12,7 @@ import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
public class RedCrabAutonomousRedBackstageEngine extends RedCrabAutonomousEngine {
@Override
public void setup() {
RedCrabMinibot robot = new RedCrabMinibot(true);
blackboardSet("clawArmPower", 0.0);
robot = new RedCrabMinibot(true);
addTask(new ClawArmTask(robot));
setupFromConfig(

2
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmMove.java
View File

@@ -31,7 +31,7 @@ public class ClawArmMove extends CyberarmState {
int tolerance = robot.clawArm.getTargetPositionTolerance();
int position = robot.clawArm.getCurrentPosition();
if (Utilities.isBetween(position, position - tolerance, position + tolerance) || runTime() >= timeoutMS) {
if (Utilities.isBetween(robot.clawArm.getTargetPosition(), position - tolerance, position + tolerance) || runTime() >= timeoutMS) {
this.finished();
}
}

25
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Move.java
View File

@@ -1,5 +1,7 @@
package dev.cyberarm.minibots.red_crab.states;
import android.util.Log;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.util.Range;
@@ -27,13 +29,21 @@ public class Move extends CyberarmState {
this.lerpMM_DOWN = robot.config.variable(groupName, actionName, "lerpMM_DOWN").value();
this.toleranceMM = robot.config.variable(groupName, actionName, "toleranceMM").value();
this.maxVelocityMM = robot.config.variable(groupName, actionName, "maxVelocityMM").value();
this.minVelocityMM = robot.config.variable(groupName, actionName, "minVelocityMM").value();
double maxVelocityMM = robot.config.variable(groupName, actionName, "maxVelocityMM").value();
double minVelocityMM = robot.config.variable(groupName, actionName, "minVelocityMM").value();
this.strafe = robot.config.variable(groupName, actionName, "strafe").value();
this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value();
if (distanceMM < 0) {
this.maxVelocityMM = Math.abs(maxVelocityMM) * -1;
this.minVelocityMM = Math.abs(minVelocityMM) * -1;
} else {
this.maxVelocityMM = Math.abs(maxVelocityMM);
this.minVelocityMM = Math.abs(minVelocityMM);
}
// Validate distance and lerp distances
if (lerpMM_UP == 0 && lerpMM_DOWN == 0) { return; }
@@ -94,6 +104,9 @@ public class Move extends CyberarmState {
robot.backLeft.setTargetPosition(distanceTicks);
robot.backRight.setTargetPosition(distanceTicks);
}
// Explicitly reset cached hub data, fixes Move state erroneous instantly finishing- sometimes.
Utilities.hubsClearBulkReadCache(engine.hardwareMap);
}
@Override
@@ -122,14 +135,17 @@ public class Move extends CyberarmState {
double velocity = lerpVelocity(travelledDistance);
double angleDiff = Utilities.angleDiff(initialHeadingDegrees, Utilities.facing(robot.imu));
if (maxVelocityMM < 0)
angleDiff = -1 * angleDiff;
double leftVelocity = velocity;
double rightVelocity = velocity;
// use +10% of power at 7 degrees of error to correct angle
double correctiveVelocity = Utilities.lerp(0.0, 1.0, angleDiff / 7.0) * (velocity * 0.1);
if (angleDiff > -0.5) {
if (angleDiff < -0.5) {
leftVelocity += correctiveVelocity;
} else if (angleDiff < 0.5) {
} else if (angleDiff > 0.5) {
rightVelocity += correctiveVelocity;
}
@@ -155,7 +171,6 @@ public class Move extends CyberarmState {
double velocity = lerpVelocity(travelledDistance);
double angleDiff = Utilities.angleDiff(initialHeadingDegrees, Utilities.facing(robot.imu));
double frontVelocity = velocity;
double backVelocity = velocity;
// use +40% of power at 7 degrees of error to correct angle

16
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathEnactor.java
View File

@@ -1,17 +1,31 @@
package dev.cyberarm.minibots.red_crab.states;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfigurationError;
import dev.cyberarm.engine.V2.CyberarmState;
import dev.cyberarm.engine.V2.Utilities;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
public class PathEnactor extends CyberarmState {
private final RedCrabMinibot robot;
private String pathGroupName;
private int forcePath;
public PathEnactor(RedCrabMinibot robot, String groupName, String actionName) {
this.robot = robot;
try {
this.forcePath = robot.config.variable(groupName, actionName, "forcePath").value();
} catch (TimeCraftersConfigurationError e) {
this.forcePath = -1;
}
}
@Override
public void start() {
// FORCE PATH FOR DEBUGGING
if (Utilities.isBetween(forcePath, 0, 2))
engine.blackboardSet("autonomousPath", forcePath);
String path;
switch (engine.blackboardGetInt("autonomousPath")) {
case 1:
@@ -25,7 +39,7 @@ public class PathEnactor extends CyberarmState {
break;
}
this.pathGroupName = String.format("AutonomousPixelPath_%s", path);
this.pathGroupName = String.format("Autonomous_SpikePath_%s", path);
}
@Override

64
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathSelector.java
View File

@@ -18,7 +18,6 @@ public class PathSelector extends CyberarmState {
private final int timeoutMS;
private final int path;
private final double minConfidence;
private List<Recognition> recognitions = new ArrayList<>();
public PathSelector(RedCrabMinibot robot, String groupName, String actionName) {
this.robot = robot;
@@ -34,40 +33,27 @@ public class PathSelector extends CyberarmState {
robot.visionPortal = VisionPortal.easyCreateWithDefaults(
engine.hardwareMap.get(WebcamName.class, robot.webcamName), robot.teamProp);
// robot.tfPixel.setClippingMargins(0, 0, 0, 0);
// robot.tfPixel.setMinResultConfidence((float) minConfidence);
engine.blackboardSet("autonomousPath", path);
}
@Override
public void exec() {
if (engine.blackboardGetInt("autonomousPath") != 0) {
this.finished();
return;
}
// recognitions = robot.tfPixel.getRecognitions();
// for (Recognition recognition : recognitions) {
// double x = (recognition.getLeft() + recognition.getRight()) / 2;
// double y = (recognition.getTop() + recognition.getBottom()) / 2;
//
// if (recognition.getLabel().equals("pixel")) {
// engine.blackboardSet("autonomousPath", path);
// }
// }
switch (robot.teamProp.getLocation()) {
case LEFT:
engine.blackboardSet("autonomousPath", 0);
break;
case CENTER:
engine.blackboardSet("autonomousPath", 1);
break;
case RIGHT:
engine.blackboardSet("autonomousPath", 2);
break;
// If we've got enough Saturation for our min confidence then do the needful.
if (robot.teamProp.getHighestSaturation() >= minConfidence) {
switch (robot.teamProp.getLocation()) {
case LEFT:
engine.blackboardSet("autonomousPath", 0);
break;
case CENTER:
engine.blackboardSet("autonomousPath", 1);
break;
case RIGHT:
engine.blackboardSet("autonomousPath", 2);
break;
}
} else
{
engine.blackboardSet("autonomousPath", path); // min confidence not meant, default to center.
}
if (runTime() >= timeoutMS) {
@@ -85,19 +71,9 @@ public class PathSelector extends CyberarmState {
public void telemetry() {
engine.telemetry.addLine();
// engine.telemetry.addData("Saturation", robot.spikeMark.getSaturation());
// engine.telemetry.addData("# Objects Detected", recognitions.size());
//
// for(Recognition recognition : recognitions) {
// double x = (recognition.getLeft() + recognition.getRight()) / 2;
// double y = (recognition.getTop() + recognition.getBottom()) / 2;
//
// engine.telemetry.addLine();
//
// engine.telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100);
// engine.telemetry.addData("- Position", "%.0f / %.0f", x, y);
// engine.telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight());
// }
engine.telemetry.addData("Location", robot.teamProp.getLocation());
engine.telemetry.addData("Saturation Left", robot.teamProp.getSaturationLeft());
engine.telemetry.addData("Saturation Center", robot.teamProp.getSaturationCenter());
engine.telemetry.addData("Saturation Right", robot.teamProp.getSaturationRight());
}
}

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

@@ -209,17 +209,6 @@ public class Pilot extends CyberarmState {
robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_COLLECT_POSITION);
break;
}
if (clawArmPosition == RedCrabMinibot.ClawArm_COLLECT &&
robot.clawArm.getCurrentPosition() >= Utilities.motorAngleToTicks(
RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO,
RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE) - 5.0) {
robot.clawArm.setPower(0);
} else {
robot.clawArm.setPower(RedCrabMinibot.CLAW_ARM_MAX_SPEED);
}
}

3
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniYTeleOPBot.java
View File

@@ -30,7 +30,8 @@ public class MiniYTeleOPBot extends Robot {
this.hardwareMap = CyberarmEngine.instance.hardwareMap;
CyberarmEngine engine = CyberarmEngine.instance;
configuration = new TimeCraftersConfiguration("Minibot Yellow");
// configuration = new TimeCraftersConfiguration("Minibot Yellow");
configuration = new TimeCraftersConfiguration();
imu = engine.hardwareMap.get(IMU.class, "imu");

5
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java
View File

@@ -19,8 +19,7 @@ public class SodiPizzaMinibotObject extends Robot {
public HardwareMap hardwareMap;
public DcMotor leftFront, rightFront, leftBack, rightBack;
// public Servo shoulder, gripper;
public Servo launcher;
public Servo shoulder, gripper, launcher;
public IMU imu;
public Rev2mDistanceSensor distSensor;
private String string;
@@ -71,7 +70,7 @@ public class SodiPizzaMinibotObject extends Robot {
rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
//Servo Defining
// shoulder = engine.hardwareMap.servo.get("arm");
shoulder = engine.hardwareMap.servo.get("shoulder");
// gripper = engine.hardwareMap.servo.get("gripper");
launcher = engine.hardwareMap.servo.get("launcher");

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

@@ -43,6 +43,7 @@ public class SodiPizzaTeleOPState extends CyberarmState {
public float getApproxObjPos() {
if (System.currentTimeMillis() - lastDistRead >= 500) {
/*Pseudocode: take objData1, wait, take 2, wait, take 3*/
}
approxObjPos = (objData1 + objData2 + objData3)/3;
return approxObjPos;
@@ -52,6 +53,9 @@ public class SodiPizzaTeleOPState extends CyberarmState {
public void telemetry() {
engine.telemetry.addData("Launcher Servo: ", robot.launcher.getPosition());
engine.telemetry.addData("Drone Launched?", droneLaunched);
engine.telemetry.addLine();
engine.telemetry.addData("Arm servo position", robot.shoulder.getPosition());
}
@Override
@@ -78,52 +82,29 @@ public class SodiPizzaTeleOPState extends CyberarmState {
@Override
public void exec() {
if (Math.abs(engine.gamepad1.left_stick_y) < minInput &&
Math.abs(engine.gamepad1.left_stick_x) < minInput &&
Math.abs(engine.gamepad1.right_stick_x) < minInput){
double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed!
double x = engine.gamepad1.left_stick_x;
double rx = engine.gamepad1.right_stick_x;
double lbPower = (y - x + rx);
double rbPower = (y + x - rx);
double lfPower = (y + x + rx);
double rfPower = (y - x - rx);
robot.leftFront.setPower(lfPower * drivePower);
robot.leftBack.setPower(lbPower * drivePower);
robot.rightFront.setPower(rfPower * drivePower);
robot.rightBack.setPower(rbPower * drivePower);
if (engine.gamepad1.left_stick_x > 0.1) {
robot.leftBack.setPower(lbPower);
robot.rightBack.setPower(rbPower);
robot.leftFront.setPower(lfPower);
robot.rightFront.setPower(rfPower);
drivePower = 0;
robot.leftFront.setPower(drivePower);
robot.rightFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
robot.rightBack.setPower(drivePower);
}
if (Math.abs(yTransitPercent) > 0.01) {
percentDenom = 100;
} else {
percentDenom = 0;
}
if (Math.abs(xTransitPercent) > 0.01) {
percentDenom = percentDenom + 100;
}
if (Math.abs(rotPercent) > 0.01) {
percentDenom = percentDenom + 100;
}
yTransitPercent = engine.gamepad1.left_stick_y * 100;
xTransitPercent = engine.gamepad1.left_stick_x * 100;
rotPercent = engine.gamepad1.right_stick_x * -100;
lfPower = ((yTransitPercent + -xTransitPercent + rotPercent) / percentDenom);
robot.leftFront.setPower(lfPower);
rfPower = ((yTransitPercent + xTransitPercent + -rotPercent) / percentDenom);
robot.rightFront.setPower(rfPower);
lbPower = ((yTransitPercent + xTransitPercent + rotPercent) / percentDenom);
robot.leftBack.setPower(lbPower);
rbPower = ((yTransitPercent + -xTransitPercent + -rotPercent) / percentDenom);
robot.rightBack.setPower(rbPower);
if (engine.gamepad2.left_stick_button) {
if (System.currentTimeMillis() - lastMoveTime >= 200) {
@@ -149,6 +130,18 @@ public class SodiPizzaTeleOPState extends CyberarmState {
}
}
if (engine.gamepad2.left_stick_y > 0.1) {
if (System.currentTimeMillis() - lastMoveTime >= 200) {
robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
lastMoveTime = System.currentTimeMillis();
}
} else if (engine.gamepad2.left_stick_y < -0.1) {
if (System.currentTimeMillis() - lastMoveTime >= 200) {
robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
lastMoveTime = System.currentTimeMillis();
}
}
// // This moves the arm to Collect position, which is at servo position 0.00.
// if (engine.gamepad2.a && !engine.gamepad2.start) {
// armPos = 1;