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-06 00:59:52 -06:00
parent 3327860e5d 9a6487a368
commit 5f2bb56fed
7 changed files with 430 additions and 387 deletions
Download Patch File Download Diff File Expand all files Collapse all files

182
TeamCode/src/main/java/dev/cyberarm/minibots/yellow/YellowMinibot.java
View File

@@ -3,8 +3,14 @@ package dev.cyberarm.minibots.yellow;
import com.arcrobotics.ftclib.hardware.motors.MotorEx;
import com.arcrobotics.ftclib.hardware.motors.MotorGroup;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.hardware.rev.RevTouchSensor;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.TouchSensor;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
@@ -14,46 +20,88 @@ import dev.cyberarm.engine.V2.CyberarmEngine;
public class YellowMinibot {
private final CyberarmEngine engine;
public final TimeCraftersConfiguration config;
public MotorEx leftFront, rightFront, leftBack, rightBack;
public MotorGroup left, right;
public IMU imu;
public CRServo droneLauncher;
public final double wheelCircumference, distancePerTick, imuAngleOffset, initialFacing;
public TimeCraftersConfiguration config;
public final DcMotorEx backLeft, frontLeft, frontRight, backRight;
public final IMU imu;
public final Servo leftClaw, rightClaw, droneLatch;
public final CRServo arm;
public final TouchSensor armEndStopLeft, armEndStopRight;
public double DRIVETRAIN_WHEEL_DIAMETER_MM = 90.0;
public int DRIVETRAIN_MOTOR_TICKS = 4;
public double DRIVETRAIN_MOTOR_GEAR_RATIO = 72.0;
public double DRIVETRAIN_MAX_VELOCITY_MM = 305.0;
public double LEFT_CLAW_CLOSED_POSITION = 0.0;
public double LEFT_CLAW_OPEN_POSITION = 0.0;
public double RIGHT_CLAW_CLOSED_POSITION = 0.0;
public double RIGHT_CLAW_OPEN_POSITION = 0.0;
public double DRONE_LATCH_INITIAL_POSITION = 0.0;
public double DRONE_LATCH_LAUNCH_POSITION = 0.0;
public YellowMinibot(CyberarmEngine engine) {
this.engine = engine;
this.config = new TimeCraftersConfiguration("Vexy_2023");
reloadConfig();
leftFront = new MotorEx(engine.hardwareMap, "leftFront");
rightFront = new MotorEx(engine.hardwareMap, "rightFront");
backLeft = (DcMotorEx) engine.hardwareMap.dcMotor.get("backLeft");
frontLeft = (DcMotorEx) engine.hardwareMap.dcMotor.get("frontLeft");
frontRight = (DcMotorEx) engine.hardwareMap.dcMotor.get("frontRight");
backRight = (DcMotorEx) engine.hardwareMap.dcMotor.get("backRight");
leftBack = new MotorEx(engine.hardwareMap, "leftBack");
rightBack = new MotorEx(engine.hardwareMap, "rightBack");
backLeft.setDirection(DcMotorSimple.Direction.FORWARD);
frontLeft.setDirection(DcMotorSimple.Direction.FORWARD);
frontRight.setDirection(DcMotorSimple.Direction.FORWARD);
backRight.setDirection(DcMotorSimple.Direction.FORWARD);
wheelCircumference = Math.PI * (50.0 * 2); //wheelRadius * wheelRadius;
distancePerTick = 288 / wheelCircumference; // raw motor encoder * gear ratio
backLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
frontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
backRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
leftFront.setDistancePerPulse(distancePerTick);
rightFront.setDistancePerPulse(distancePerTick);
leftBack.setDistancePerPulse(distancePerTick);
rightBack.setDistancePerPulse(distancePerTick);
backLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
frontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
frontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
left = new MotorGroup(leftFront, leftBack);
right = new MotorGroup(rightFront, rightBack);
leftClaw = engine.hardwareMap.servo.get("leftClaw"); /// Port 0
rightClaw = engine.hardwareMap.servo.get("rightClaw"); /// Port 1
arm = engine.hardwareMap.crservo.get("arm"); /// Port 2
arm.setDirection(DcMotorSimple.Direction.REVERSE);
droneLatch = engine.hardwareMap.servo.get("droneLatch"); /// Port 3
imu = engine.hardwareMap.get(IMU.class, "imu");
armEndStopLeft = engine.hardwareMap.touchSensor.get("armEndStopLeft");
armEndStopRight = engine.hardwareMap.touchSensor.get("armEndStopRight");
imu = engine.hardwareMap.get(IMU.class, "imu"); // Embedded | IC2 port 0
IMU.Parameters parameters = new IMU.Parameters(
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP,
RevHubOrientationOnRobot.UsbFacingDirection.RIGHT));
imu.initialize(parameters);
}
imuAngleOffset = 0;
initialFacing = facing();
public void reloadConfig() {
this.config = new TimeCraftersConfiguration("cyberarm_Vexy_Yellow");
droneLauncher = engine.hardwareMap.crservo.get("droneLauncher"); /// Port 5
loadConstants();
}
public void loadConstants() {
DRIVETRAIN_WHEEL_DIAMETER_MM = config.variable("Robot", "Drivetrain_Tuning", "wheel_diameter_mm").value();
DRIVETRAIN_MOTOR_TICKS = config.variable("Robot", "Drivetrain_Tuning", "motor_ticks").value();
DRIVETRAIN_MOTOR_GEAR_RATIO = config.variable("Robot", "Drivetrain_Tuning", "motor_gear_ratio").value();
DRIVETRAIN_MAX_VELOCITY_MM = config.variable("Robot", "Drivetrain_Tuning", "max_velocity_mm").value();
LEFT_CLAW_CLOSED_POSITION = config.variable("Robot", "LeftClaw_Tuning", "open_position").value();
LEFT_CLAW_OPEN_POSITION = config.variable("Robot", "LeftClaw_Tuning", "closed_position").value();
RIGHT_CLAW_OPEN_POSITION = config.variable("Robot", "RightClaw_Tuning", "open_position").value();
RIGHT_CLAW_CLOSED_POSITION = config.variable("Robot", "RightClaw_Tuning", "closed_position").value();
DRONE_LATCH_INITIAL_POSITION = config.variable("Robot", "DroneLatch_Tuning", "initial_position").value();
DRONE_LATCH_LAUNCH_POSITION = config.variable("Robot", "DroneLatch_Tuning", "launch_position").value();
}
public void standardTelemetry() {
@@ -61,35 +109,45 @@ public class YellowMinibot {
engine.telemetry.addLine("Motors");
engine.telemetry.addData(
"Left Front",
"Front Left",
"Power: %.2f, Current: %.2f mAmp, Position: %d",
leftFront.motorEx.getPower(),
leftFront.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
leftFront.motorEx.getCurrentPosition());
frontLeft.getPower(),
frontLeft.getCurrent(CurrentUnit.MILLIAMPS),
frontLeft.getCurrentPosition());
engine.telemetry.addData(
"Right Front",
"Front Right",
"Power: %.2f, Current: %.2f mAmp, Position: %d",
rightFront.motorEx.getPower(),
rightFront.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
rightFront.motorEx.getCurrentPosition());
frontRight.getPower(),
frontRight.getCurrent(CurrentUnit.MILLIAMPS),
frontRight.getCurrentPosition());
engine.telemetry.addData(
"Left Back",
"Back Left",
"Power: %.2f, Current: %.2f mAmp, Position: %d",
leftBack.motorEx.getPower(),
leftBack.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
leftBack.motorEx.getCurrentPosition());
backLeft.getPower(),
backLeft.getCurrent(CurrentUnit.MILLIAMPS),
backLeft.getCurrentPosition());
engine.telemetry.addData(
"Right Back",
"Back Right",
"Power: %.2f, Current: %.2f mAmp, Position: %d",
rightBack.motorEx.getPower(),
rightBack.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
rightBack.motorEx.getCurrentPosition());
backRight.getPower(),
backRight.getCurrent(CurrentUnit.MILLIAMPS),
backRight.getCurrentPosition());
engine.telemetry.addLine();
engine.telemetry.addLine("Servos");
engine.telemetry.addData("droneLauncher", droneLauncher.getPower());
engine.telemetry.addData("left claw", leftClaw.getPosition());
engine.telemetry.addData("right claw", rightClaw.getPosition());
engine.telemetry.addData("arm", arm.getPower());
engine.telemetry.addData("droneLatch", droneLatch.getPosition());
engine.telemetry.addLine();
engine.telemetry.addData("arm endstop left", armEndStopLeft.isPressed());
engine.telemetry.addData("arm endstop right", armEndStopRight.isPressed());
engine.telemetry.addLine();
}
public void teleopTelemetry() {
@@ -97,46 +155,4 @@ public class YellowMinibot {
engine.telemetry.addLine();
}
public double distanceMM(int ticks) {
return distancePerTick * ticks;
}
public double initialFacing() {
return initialFacing;
}
public double facing() {
double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
return (((imuDegrees + 360.0) % 360.0) + imuAngleOffset) % 360.0;
}
public double heading() {
return AngleUnit.normalizeRadians(-facing() * Math.PI / 180.0);
// return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
}
public double turnRate() {
return imu.getRobotAngularVelocity(AngleUnit.DEGREES).yRotationRate; // NOTE: UNTESTED
}
public boolean isBetween(double value, double min, double max) {
return value >= min && value <= max;
}
// Adapted from: https://github.com/gosu/gosu/blob/980d64de2ce52e4b16fdd5cb9c9e11c8bbb80671/src/Math.cpp#L38
public double angleDiff(double from, double to) {
double value = (to - from + 180);
double fmod = (value - 0.0) % (360.0 - 0.0);
return (fmod < 0 ? fmod + 360.0 : fmod + 0.0) - 180;
}
public double lerp(double min, double max, double t)
{
return min + (max - min) * t;
}
}
}

107
TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Pilot.java
View File

@@ -1,40 +1,43 @@
package dev.cyberarm.minibots.yellow.states;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import dev.cyberarm.engine.V2.CyberarmState;
import dev.cyberarm.engine.V2.Utilities;
import dev.cyberarm.minibots.yellow.YellowMinibot;
public class Pilot extends CyberarmState {
final YellowMinibot robot;
private boolean leftClawOpen, rightClawOpen;
public Pilot(YellowMinibot robot) {
this.robot = robot;
this.leftClawOpen = false;
this.rightClawOpen = false;
}
@Override
public void exec() {
/// --- IMU Reset --- ///
if (engine.gamepad1.guide) {
robot.imu.resetYaw();
}
drivetrain();
armController();
clawControllers();
droneLatchController();
}
/// --- DRONE --- ///
if (engine.gamepad1.y) {
robot.droneLauncher.setPower(1.0);
} else if (engine.gamepad1.a) {
robot.droneLauncher.setPower(0.0);
}
/// --- DRIVE --- ///
private void drivetrain()
{
// robot.left.set(engine.gamepad1.left_stick_y);
// robot.right.set(engine.gamepad1.right_stick_y);
// https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html#field-centric
double y = engine.gamepad1.left_stick_y; // Remember, Y stick value is reversed;
double x = -engine.gamepad1.left_stick_x;
double rx = -engine.gamepad1.right_stick_x;
double y = -engine.gamepad1.left_stick_y; // Remember, Y stick value is reversed;
double x = engine.gamepad1.left_stick_x;
double rx = engine.gamepad1.right_stick_x;
double botHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
@@ -53,16 +56,72 @@ public class Pilot extends CyberarmState {
double frontRightPower = (rotY - rotX - rx) / denominator;
double backRightPower = (rotY + rotX - rx) / denominator;
// frontLeftPower *= 24.0;
// backLeftPower *= 24.0;
// frontRightPower *= 24.0;
// backRightPower *= 24.0;
double velocity = Utilities.unitToTicks(
robot.DRIVETRAIN_MOTOR_TICKS,
robot.DRIVETRAIN_MOTOR_GEAR_RATIO,
robot.DRIVETRAIN_WHEEL_DIAMETER_MM,
DistanceUnit.MM,
robot.DRIVETRAIN_MAX_VELOCITY_MM);
double maxPower = 1.0;
robot.backLeft.setVelocity(frontLeftPower * velocity);
robot.frontLeft.setVelocity(backLeftPower * velocity);
robot.frontRight.setVelocity(frontRightPower * velocity);
robot.backRight.setVelocity(backRightPower * velocity);
}
robot.leftFront.motorEx.setPower(frontLeftPower * maxPower);
robot.leftBack.motorEx.setPower(backLeftPower * maxPower);
robot.rightFront.motorEx.setPower(frontRightPower * maxPower);
robot.rightBack.motorEx.setPower(backRightPower * maxPower);
private void armController()
{
double armPower = engine.gamepad1.right_trigger - engine.gamepad1.left_trigger;
if (armPower > 0) {
if (robot.armEndStopLeft.isPressed() || robot.armEndStopRight.isPressed())
armPower = 0;
}
robot.arm.setPower(armPower);
}
private void clawControllers()
{
robot.leftClaw.setPosition(leftClawOpen ? robot.LEFT_CLAW_OPEN_POSITION : robot.LEFT_CLAW_CLOSED_POSITION);
robot.rightClaw.setPosition(rightClawOpen ? robot.RIGHT_CLAW_OPEN_POSITION : robot.RIGHT_CLAW_CLOSED_POSITION);
}
private void droneLatchController()
{}
@Override
public void telemetry() {
robot.standardTelemetry();
robot.teleopTelemetry();
}
@Override
public void buttonDown(Gamepad gamepad, String button) {
if (gamepad == engine.gamepad1)
{
switch (button) {
case "guide":
/// --- IMU Reset --- ///
robot.imu.resetYaw();
break;
case "start":
robot.reloadConfig();
break;
case "left_bumper":
leftClawOpen = !leftClawOpen;
break;
case "right_bumper":
rightClawOpen = !rightClawOpen;
break;
case "y":
/// DRONE LATCH ///
robot.droneLatch.setPosition(robot.DRONE_LATCH_LAUNCH_POSITION);
break;
case "a":
/// DRONE LATCH ///
robot.droneLatch.setPosition(robot.DRONE_LATCH_INITIAL_POSITION);
break;
}
}
}
}

62
TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Rotate.java
View File

@@ -47,40 +47,40 @@ public class Rotate extends CyberarmState {
@Override
public void exec() {
if (Math.abs(robot.angleDiff(robot.facing(), heading)) <= tolerance) {
robot.left.set(0);
robot.right.set(0);
setHasFinished(true);
return;
}
if (runTime() >= timeoutMS) {
robot.left.set(0);
robot.right.set(0);
setHasFinished(true);
}
double angleDiff = robot.angleDiff(robot.facing() + robot.turnRate(), heading);
double velocity = robot.lerp(minVelocity, maxVelocity, Range.clip(Math.abs(angleDiff) / 90.0, 0.0, 1.0));
if (angleDiff > 0) {
robot.left.set(-velocity);
robot.right.set(velocity);
} else {
robot.left.set(velocity);
robot.right.set(-velocity);
}
// if (Math.abs(robot.angleDiff(robot.facing(), heading)) <= tolerance) {
// robot.left.set(0);
// robot.right.set(0);
//
// setHasFinished(true);
//
// return;
// }
//
// if (runTime() >= timeoutMS) {
// robot.left.set(0);
// robot.right.set(0);
//
// setHasFinished(true);
// }
//
// double angleDiff = robot.angleDiff(robot.facing() + robot.turnRate(), heading);
// double velocity = robot.lerp(minVelocity, maxVelocity, Range.clip(Math.abs(angleDiff) / 90.0, 0.0, 1.0));
//
// if (angleDiff > 0) {
// robot.left.set(-velocity);
// robot.right.set(velocity);
// } else {
// robot.left.set(velocity);
// robot.right.set(-velocity);
// }
}
@Override
public void telemetry() {
engine.telemetry.addLine();
engine.telemetry.addData("Robot Heading", robot.facing());
engine.telemetry.addData("Robot Target Heading", heading);
engine.telemetry.addData("Robot Angle Diff", robot.angleDiff(robot.facing() + robot.turnRate(), heading));
engine.telemetry.addData("Robot Turn Rate", robot.turnRate());
// engine.telemetry.addLine();
// engine.telemetry.addData("Robot Heading", robot.facing());
// engine.telemetry.addData("Robot Target Heading", heading);
// engine.telemetry.addData("Robot Angle Diff", robot.angleDiff(robot.facing() + robot.turnRate(), heading));
// engine.telemetry.addData("Robot Turn Rate", robot.turnRate());
}
}

34
TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/StrafeMove.java
View File

@@ -38,26 +38,26 @@ public class StrafeMove extends CyberarmState {
@Override
public void start() {
robot.leftFront.setTargetDistance(distanceMM);
robot.left.setPositionTolerance(tolerance);
robot.right.setPositionTolerance(tolerance);
double motorVelocity = (distanceMM < 0 ? velocity * -1 : velocity);
robot.leftFront.set(motorVelocity);
robot.rightFront.set(-motorVelocity);
robot.leftBack.set(-motorVelocity);
robot.rightBack.set(motorVelocity);
// robot.leftFront.setTargetDistance(distanceMM);
//
// robot.left.setPositionTolerance(tolerance);
// robot.right.setPositionTolerance(tolerance);
//
// double motorVelocity = (distanceMM < 0 ? velocity * -1 : velocity);
//
// robot.leftFront.set(motorVelocity);
// robot.rightFront.set(-motorVelocity);
//
// robot.leftBack.set(-motorVelocity);
// robot.rightBack.set(motorVelocity);
}
@Override
public void exec() {
if (robot.left.atTargetPosition() || Math.abs(robot.leftFront.getDistance()) >= Math.abs(distanceMM) || runTime() >= timeoutMS) {
robot.left.set(0);
robot.right.set(0);
setHasFinished(true);
}
// if (robot.left.atTargetPosition() || Math.abs(robot.leftFront.getDistance()) >= Math.abs(distanceMM) || runTime() >= timeoutMS) {
// robot.left.set(0);
// robot.right.set(0);
// setHasFinished(true);
// }
}
}

76
TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/TankMove.java
View File

@@ -43,51 +43,51 @@ public class TankMove extends CyberarmState {
@Override
public void start() {
robot.left.resetEncoder();
robot.right.resetEncoder();
robot.left.setTargetDistance(leftDistanceMM);
robot.right.setTargetDistance(rightDistanceMM);
robot.left.setPositionTolerance(tolerance);
robot.right.setPositionTolerance(tolerance);
robot.left.set(leftVelocity);
robot.right.set(rightVelocity);
// robot.left.resetEncoder();
// robot.right.resetEncoder();
//
// robot.left.setTargetDistance(leftDistanceMM);
// robot.right.setTargetDistance(rightDistanceMM);
//
// robot.left.setPositionTolerance(tolerance);
// robot.right.setPositionTolerance(tolerance);
//
// robot.left.set(leftVelocity);
// robot.right.set(rightVelocity);
}
@Override
public void exec() {
if (robot.left.atTargetPosition() || Math.abs(robot.leftFront.getDistance()) >= Math.abs(leftDistanceMM)) {
robot.left.set(0);
}
if (robot.right.atTargetPosition() || Math.abs(robot.rightFront.getDistance()) >= Math.abs(leftDistanceMM)) {
robot.right.set(0);
}
if (
(robot.left.atTargetPosition() && robot.right.atTargetPosition()) ||
(Math.abs(robot.leftFront.getDistance()) >= Math.abs(leftDistanceMM) && robot.right.atTargetPosition() || Math.abs(robot.rightFront.getDistance()) >= Math.abs(leftDistanceMM)) ||
runTime() >= timeoutMS) {
robot.left.set(0);
robot.right.set(0);
setHasFinished(true);
}
// if (robot.left.atTargetPosition() || Math.abs(robot.leftFront.getDistance()) >= Math.abs(leftDistanceMM)) {
// robot.left.set(0);
// }
// if (robot.right.atTargetPosition() || Math.abs(robot.rightFront.getDistance()) >= Math.abs(leftDistanceMM)) {
// robot.right.set(0);
// }
//
// if (
// (robot.left.atTargetPosition() && robot.right.atTargetPosition()) ||
// (Math.abs(robot.leftFront.getDistance()) >= Math.abs(leftDistanceMM) && robot.right.atTargetPosition() || Math.abs(robot.rightFront.getDistance()) >= Math.abs(leftDistanceMM)) ||
// runTime() >= timeoutMS) {
// robot.left.set(0);
// robot.right.set(0);
// setHasFinished(true);
// }
}
@Override
public void telemetry() {
engine.telemetry.addLine();
engine.telemetry.addData("Left distance", robot.leftFront.getDistance());
engine.telemetry.addData("Left position", robot.left.getPositions().get(0));
engine.telemetry.addData("Left speed", robot.left.getSpeeds().get(0));
engine.telemetry.addData("Left velocity", robot.left.getVelocity());
engine.telemetry.addLine();
engine.telemetry.addData("Right distance", robot.rightFront.getDistance());
engine.telemetry.addData("Right position", robot.right.getPositions().get(0));
engine.telemetry.addData("Right speed", robot.right.getSpeeds().get(0));
engine.telemetry.addData("Right velocity", robot.right.getVelocity());
// engine.telemetry.addLine();
//
// engine.telemetry.addData("Left distance", robot.leftFront.getDistance());
// engine.telemetry.addData("Left position", robot.left.getPositions().get(0));
// engine.telemetry.addData("Left speed", robot.left.getSpeeds().get(0));
// engine.telemetry.addData("Left velocity", robot.left.getVelocity());
// engine.telemetry.addLine();
//
// engine.telemetry.addData("Right distance", robot.rightFront.getDistance());
// engine.telemetry.addData("Right position", robot.right.getPositions().get(0));
// engine.telemetry.addData("Right speed", robot.right.getSpeeds().get(0));
// engine.telemetry.addData("Right velocity", robot.right.getVelocity());
}
}

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

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

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

@@ -26,7 +26,9 @@ public class SodiPizzaTeleOPState extends CyberarmState {
public final double minInput = 0.1 /* <- Minimum input from stick to send command */;
public double lastToldAngle /* <- The angle the bot was last told to stop at */, proportion, integral, derivative;
public float approxObjPos /* <- Approximate distance away the nearest obstruction is */, objData1, objData2, objData3;
private int armPos, objectPos;
private double lfPower, rfPower, lbPower, rbPower;
private float yTransitPercent, xTransitPercent, rotPercent, percentDenom;
private int objectPos;
private boolean droneLaunched;
private char buttonPressed;
private GamepadChecker gp1checker, gp2checker;
@@ -48,12 +50,6 @@ public class SodiPizzaTeleOPState extends CyberarmState {
@Override
public void telemetry() {
engine.telemetry.addData("Arm should be at Position ", armPos);
engine.telemetry.addData("Arm servo is at ", robot.shoulder.getPosition());
engine.telemetry.addData("Button Pressed = ", buttonPressed);
engine.telemetry.addLine();
engine.telemetry.addData("Launcher Servo: ", robot.launcher.getPosition());
engine.telemetry.addData("Drone Launched?", droneLaunched);
}
@@ -77,15 +73,18 @@ public class SodiPizzaTeleOPState extends CyberarmState {
gp2checker = new GamepadChecker(engine, engine.gamepad2);
lastMoveTime = System.currentTimeMillis();
armPos = 0;
lastDistRead = System.currentTimeMillis();
}
@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) /* <- input from ONLY left stick y means to move forward or backward */{
Math.abs(engine.gamepad1.right_stick_x) < minInput){
drivePower = 0;
robot.leftFront.setPower(drivePower);
@@ -94,69 +93,37 @@ public class SodiPizzaTeleOPState extends CyberarmState {
robot.rightBack.setPower(drivePower);
}
if (robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) > lastToldAngle + 0.5) {
if (Math.abs(engine.gamepad1.right_stick_x) > minInput &&
Math.abs(engine.gamepad1.left_stick_y) > minInput) {
robot.rightFront.setPower(robot.leftFront.getPower() * 0.8);
robot.rightBack.setPower(robot.leftBack.getPower() * 0.8);
}
} else if (robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) < lastToldAngle - 0.5 &&
Math.abs(engine.gamepad1.left_stick_y) > minInput) {
robot.leftFront.setPower(robot.rightFront.getPower() * 0.8);
robot.leftBack.setPower(robot.rightBack.getPower() * 0.8);
if (Math.abs(yTransitPercent) > 0.01) {
percentDenom = 100;
} else {
robot.leftFront.setPower(drivePower);
robot.rightFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
robot.rightBack.setPower(drivePower);
percentDenom = 0;
}
if (engine.gamepad1.start && !engine.gamepad1.a) /*<-- reset everything: encoders, imu, and armPos int*/ {
if (Math.abs(xTransitPercent) > 0.01) {
robot.leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.leftBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.imu.resetYaw();
percentDenom = percentDenom + 100;
}
if (Math.abs(engine.gamepad1.left_stick_y) > minInput &&
Math.abs(engine.gamepad1.left_stick_x) < minInput &&
armPos == 0/* &&
robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) > lastToldAngle - 0.5 &&
robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) < lastToldAngle + 0.5*/) {
if (Math.abs(rotPercent) > 0.01) {
drivePower = engine.gamepad1.left_stick_y;
robot.leftFront.setPower(drivePower);
robot.rightFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
robot.rightBack.setPower(drivePower);
percentDenom = percentDenom + 100;
}
yTransitPercent = engine.gamepad1.left_stick_y * 100;
xTransitPercent = engine.gamepad1.left_stick_x * 100;
rotPercent = engine.gamepad1.right_stick_x * -100;
if (Math.abs(engine.gamepad1.left_stick_x) > minInput &&
armPos == 0) {
lfPower = ((yTransitPercent + -xTransitPercent + rotPercent) / percentDenom);
robot.leftFront.setPower(lfPower);
drivePower = engine.gamepad1.left_stick_x;
robot.leftFront.setPower(-drivePower);
robot.rightFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
robot.rightBack.setPower(-drivePower);
}
rfPower = ((yTransitPercent + xTransitPercent + -rotPercent) / percentDenom);
robot.rightFront.setPower(rfPower);
if (Math.abs(engine.gamepad1.right_stick_x) > minInput &&
armPos == 0) {
lbPower = ((yTransitPercent + xTransitPercent + rotPercent) / percentDenom);
robot.leftBack.setPower(lbPower);
drivePower = engine.gamepad1.right_stick_x;
robot.leftFront.setPower(-drivePower);
robot.rightFront.setPower(drivePower);
robot.leftBack.setPower(-drivePower);
robot.rightBack.setPower(drivePower);
lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
}
rbPower = ((yTransitPercent + -xTransitPercent + -rotPercent) / percentDenom);
robot.rightBack.setPower(rbPower);
if (engine.gamepad2.left_stick_button) {
if (System.currentTimeMillis() - lastMoveTime >= 200) {
@@ -182,140 +149,140 @@ public class SodiPizzaTeleOPState extends CyberarmState {
}
}
// This moves the arm to Collect position, which is at servo position 0.00.
if (engine.gamepad2.a && !engine.gamepad2.start) {
armPos = 1;
}
if (armPos == 1) {
buttonPressed = 'a';
if (Math.abs(drivePower) > 0.25) {
drivePower = 0.15;
robot.leftFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
robot.rightFront.setPower(drivePower);
robot.rightBack.setPower(drivePower);
} else {
//if servo's position is greater than Collect position with a run-to tolerance of 0.05,
//decrement position at a rate of 0.05 per 150 milliseconds.
if (robot.shoulder.getPosition() > ARM_PRECOLLECT && System.currentTimeMillis() - lastMoveTime >= 150) {
robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
lastMoveTime = System.currentTimeMillis();
} else if (System.currentTimeMillis() - lastMoveTime >= 150) {
robot.shoulder.setPosition(ARM_COLLECT);
armPos = 0;
}
}
}
//End of code for armPos = 1
// This moves the arm to Precollect position, which is at servo position 0.05.
if (engine.gamepad2.x) {
armPos = 2;
}
if (armPos == 2) {
buttonPressed = 'x';
if (Math.abs(drivePower) > 0.25) {
drivePower = 0.15;
robot.leftFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
robot.rightFront.setPower(drivePower);
robot.rightBack.setPower(drivePower);
} else {
//if servo's position is greater than Precollect position with a run-to tolerance of 0.05,
//decrement position at a rate of 0.05 per 150 milliseconds.
if (robot.shoulder.getPosition() > ARM_PRECOLLECT + 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
lastMoveTime = System.currentTimeMillis();
}//Incrementing from Collect position is unnecessary, because Collect is within the tolerance of run-to.
else if (System.currentTimeMillis() - lastMoveTime >= 150) {
robot.shoulder.setPosition(ARM_PRECOLLECT);
armPos = 0;
}
}
}
//End of code for armPos = 2
// This moves the arm to Deliver position, which is at servo position 0.28.
if (engine.gamepad2.b && !engine.gamepad2.start) {
armPos = 3;
}
if (armPos == 3) {
buttonPressed = 'b';
if (Math.abs(drivePower) > 0.25) {
drivePower = 0.15;
robot.leftFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
robot.rightFront.setPower(drivePower);
robot.rightBack.setPower(drivePower);
} else {
//if servo's position is less than Deliver position with a run-to tolerance of 0.05,
//increment position at a rate of 0.05 per 150 milliseconds.
if (robot.shoulder.getPosition() < ARM_DELIVER - 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
lastMoveTime = System.currentTimeMillis();
}//if servo's position is greater than Deliver position with a run-to tolerance of 0.05,
//decrement position at a rate of 0.05 per 150 milliseconds.
else if (robot.shoulder.getPosition() > ARM_DELIVER + 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
lastMoveTime = System.currentTimeMillis();
} else if (System.currentTimeMillis() - lastMoveTime >= 150) {
robot.shoulder.setPosition(ARM_DELIVER);
armPos = 0;
}
}
}
//End of code for armPos = 3
// This moves the arm to Stow position, which is at servo position 0.72.
if (engine.gamepad2.y) {
armPos = 4;
}
if (armPos == 4) {
buttonPressed = 'y';
if (Math.abs(drivePower) > 0.25) {
drivePower = 0.15;
robot.leftFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
robot.rightFront.setPower(drivePower);
robot.rightBack.setPower(drivePower);
} else {
//if servo's position is less than Collect position with a run-to tolerance of 0.05,
//increment position at a rate of 0.05 per 150 milliseconds.
if (robot.shoulder.getPosition() < ARM_STOW - 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
lastMoveTime = System.currentTimeMillis();
}
//Decrementing is unnecessary, because servo is mechanically inhibited from further advancing.
else if (System.currentTimeMillis() - lastMoveTime >= 150) {
robot.shoulder.setPosition(ARM_STOW);
armPos = 0;
}
}
}
// // This moves the arm to Collect position, which is at servo position 0.00.
// if (engine.gamepad2.a && !engine.gamepad2.start) {
// armPos = 1;
// }
//
// if (armPos == 1) {
//
// buttonPressed = 'a';
//
// if (Math.abs(drivePower) > 0.25) {
// drivePower = 0.15;
// robot.leftFront.setPower(drivePower);
// robot.leftBack.setPower(drivePower);
// robot.rightFront.setPower(drivePower);
// robot.rightBack.setPower(drivePower);
// } else {
// //if servo's position is greater than Collect position with a run-to tolerance of 0.05,
// //decrement position at a rate of 0.05 per 150 milliseconds.
// if (robot.shoulder.getPosition() > ARM_PRECOLLECT && System.currentTimeMillis() - lastMoveTime >= 150) {
// robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
// lastMoveTime = System.currentTimeMillis();
// } else if (System.currentTimeMillis() - lastMoveTime >= 150) {
// robot.shoulder.setPosition(ARM_COLLECT);
// armPos = 0;
// }
//
// }
//
// }
// //End of code for armPos = 1
//
// // This moves the arm to Precollect position, which is at servo position 0.05.
// if (engine.gamepad2.x) {
// armPos = 2;
// }
//
// if (armPos == 2) {
//
// buttonPressed = 'x';
//
// if (Math.abs(drivePower) > 0.25) {
// drivePower = 0.15;
// robot.leftFront.setPower(drivePower);
// robot.leftBack.setPower(drivePower);
// robot.rightFront.setPower(drivePower);
// robot.rightBack.setPower(drivePower);
// } else {
// //if servo's position is greater than Precollect position with a run-to tolerance of 0.05,
// //decrement position at a rate of 0.05 per 150 milliseconds.
// if (robot.shoulder.getPosition() > ARM_PRECOLLECT + 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
// robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
// lastMoveTime = System.currentTimeMillis();
// }//Incrementing from Collect position is unnecessary, because Collect is within the tolerance of run-to.
// else if (System.currentTimeMillis() - lastMoveTime >= 150) {
// robot.shoulder.setPosition(ARM_PRECOLLECT);
// armPos = 0;
// }
//
// }
// }
// //End of code for armPos = 2
//
// // This moves the arm to Deliver position, which is at servo position 0.28.
// if (engine.gamepad2.b && !engine.gamepad2.start) {
// armPos = 3;
// }
//
// if (armPos == 3) {
//
// buttonPressed = 'b';
//
// if (Math.abs(drivePower) > 0.25) {
// drivePower = 0.15;
// robot.leftFront.setPower(drivePower);
// robot.leftBack.setPower(drivePower);
// robot.rightFront.setPower(drivePower);
// robot.rightBack.setPower(drivePower);
// } else {
// //if servo's position is less than Deliver position with a run-to tolerance of 0.05,
// //increment position at a rate of 0.05 per 150 milliseconds.
// if (robot.shoulder.getPosition() < ARM_DELIVER - 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
// robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
// lastMoveTime = System.currentTimeMillis();
// }//if servo's position is greater than Deliver position with a run-to tolerance of 0.05,
// //decrement position at a rate of 0.05 per 150 milliseconds.
// else if (robot.shoulder.getPosition() > ARM_DELIVER + 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
// robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
// lastMoveTime = System.currentTimeMillis();
// } else if (System.currentTimeMillis() - lastMoveTime >= 150) {
// robot.shoulder.setPosition(ARM_DELIVER);
// armPos = 0;
// }
//
// }
// }
// //End of code for armPos = 3
//
// // This moves the arm to Stow position, which is at servo position 0.72.
// if (engine.gamepad2.y) {
// armPos = 4;
// }
//
// if (armPos == 4) {
//
// buttonPressed = 'y';
//
// if (Math.abs(drivePower) > 0.25) {
// drivePower = 0.15;
// robot.leftFront.setPower(drivePower);
// robot.leftBack.setPower(drivePower);
// robot.rightFront.setPower(drivePower);
// robot.rightBack.setPower(drivePower);
// } else {
// //if servo's position is less than Collect position with a run-to tolerance of 0.05,
// //increment position at a rate of 0.05 per 150 milliseconds.
// if (robot.shoulder.getPosition() < ARM_STOW - 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
// robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
// lastMoveTime = System.currentTimeMillis();
// }
// //Decrementing is unnecessary, because servo is mechanically inhibited from further advancing.
// else if (System.currentTimeMillis() - lastMoveTime >= 150) {
// robot.shoulder.setPosition(ARM_STOW);
// armPos = 0;
// }
//
// }
// }
//End of code for armPos = 4
if (engine.gamepad2.dpad_left) {
robot.gripper.setPosition(GRIPPER_OPEN);
} else if (engine.gamepad2.dpad_right) {
robot.gripper.setPosition(GRIPPER_CLOSED);
}
// if (engine.gamepad2.dpad_left) {
// robot.gripper.setPosition(GRIPPER_OPEN);
// } else if (engine.gamepad2.dpad_right) {
// robot.gripper.setPosition(GRIPPER_CLOSED);
// }
gp1checker.update();
gp2checker.update();