mirror of
https://github.com/TimeCrafters/CenterStage
synced 2025-12-15 08:42:35 +00:00
Merge remote-tracking branch 'origin/master'
This commit is contained in:
SpencerPiha
@@ -0,0 +1,179 @@
|
||||
package dev.cyberarm.minibots.pizza;
|
||||
|
||||
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.rev.RevHubOrientationOnRobot;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.IMU;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
|
||||
public class PizzaMinibot {
|
||||
public static double GRIPPER_CLOSED = 0.333; // ~90 degrees
|
||||
public static double GRIPPER_OPEN = 0.75; // ~205 degrees
|
||||
|
||||
public static double ARM_COLLECT = 0.0; // ~? degrees
|
||||
public static double ARM_PRECOLLECT = 0.05; // ~? degrees
|
||||
public static double ARM_DELIVER = 0.28; // ~? degrees
|
||||
public static double ARM_STOW = 0.72; // ~? degrees
|
||||
public static double ARM_HOVER_5_STACK = 0.10;
|
||||
public static double ARM_HOVER_4_STACK = 0.08;
|
||||
public static double ARM_HOVER_3_STACK = 0.07;
|
||||
public static double ARM_HOVER_2_STACK = 0.06;
|
||||
public static double ARM_HOVER_1_STACK = ARM_PRECOLLECT;
|
||||
public MotorEx leftFront, rightFront, leftBack, rightBack;
|
||||
public MotorGroup left, right;
|
||||
public IMU imu;
|
||||
public Servo gripper, arm;
|
||||
public int armStackPosition = -1;
|
||||
final public TimeCraftersConfiguration config;
|
||||
final public double wheelCircumference, distancePerTick;
|
||||
|
||||
public final double imuAngleOffset, initialFacing;
|
||||
|
||||
private CyberarmEngine engine;
|
||||
|
||||
public PizzaMinibot(CyberarmEngine engine) {
|
||||
this.engine = engine;
|
||||
|
||||
this.config = new TimeCraftersConfiguration("Pizza_2023");
|
||||
|
||||
leftFront = new MotorEx(engine.hardwareMap, "leftFront");
|
||||
rightFront = new MotorEx(engine.hardwareMap, "rightFront");
|
||||
|
||||
leftBack = new MotorEx(engine.hardwareMap, "leftBack");
|
||||
rightBack = new MotorEx(engine.hardwareMap, "rightBack");
|
||||
|
||||
rightFront.motorEx.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
rightBack.motorEx.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
|
||||
leftFront.motorEx.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
leftBack.motorEx.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
|
||||
leftFront.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
|
||||
rightFront.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
|
||||
|
||||
leftBack.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
|
||||
rightBack.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
|
||||
|
||||
wheelCircumference = Math.PI * (75.0 * 4); //wheelRadius * wheelRadius; //-- times 2 is a hack... --//
|
||||
distancePerTick = (28 * 20) / wheelCircumference; // raw motor encoder * gear ratio
|
||||
|
||||
leftFront.setDistancePerPulse(distancePerTick);
|
||||
rightFront.setDistancePerPulse(distancePerTick);
|
||||
leftBack.setDistancePerPulse(distancePerTick);
|
||||
rightBack.setDistancePerPulse(distancePerTick);
|
||||
|
||||
left = new MotorGroup(leftFront, leftBack);
|
||||
right = new MotorGroup(rightFront, rightBack);
|
||||
|
||||
imu = engine.hardwareMap.get(IMU.class, "imu");
|
||||
IMU.Parameters parameters = new IMU.Parameters(
|
||||
new RevHubOrientationOnRobot(
|
||||
RevHubOrientationOnRobot.LogoFacingDirection.UP,
|
||||
RevHubOrientationOnRobot.UsbFacingDirection.LEFT));
|
||||
|
||||
imu.initialize(parameters);
|
||||
|
||||
imuAngleOffset = 0;
|
||||
initialFacing = facing();
|
||||
|
||||
gripper = engine.hardwareMap.servo.get("gripper");
|
||||
arm = engine.hardwareMap.servo.get("arm");
|
||||
|
||||
gripper.setPosition(PizzaMinibot.GRIPPER_CLOSED);
|
||||
}
|
||||
|
||||
public void standardTelemetry() {
|
||||
engine.telemetry.addLine();
|
||||
|
||||
engine.telemetry.addLine("Motors");
|
||||
engine.telemetry.addData(
|
||||
"Left Front",
|
||||
"Power: %.2f, Current: %.2f mAmp, Position: %d",
|
||||
leftFront.motorEx.getPower(),
|
||||
leftFront.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
|
||||
leftFront.motorEx.getCurrentPosition());
|
||||
engine.telemetry.addData(
|
||||
"Right Front",
|
||||
"Power: %.2f, Current: %.2f mAmp, Position: %d",
|
||||
rightFront.motorEx.getPower(),
|
||||
rightFront.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
|
||||
rightFront.motorEx.getCurrentPosition());
|
||||
engine.telemetry.addData(
|
||||
"Left Back",
|
||||
"Power: %.2f, Current: %.2f mAmp, Position: %d",
|
||||
leftBack.motorEx.getPower(),
|
||||
leftBack.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
|
||||
leftBack.motorEx.getCurrentPosition());
|
||||
engine.telemetry.addData(
|
||||
"Right Back",
|
||||
"Power: %.2f, Current: %.2f mAmp, Position: %d",
|
||||
rightBack.motorEx.getPower(),
|
||||
rightBack.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
|
||||
rightBack.motorEx.getCurrentPosition());
|
||||
|
||||
engine.telemetry.addLine();
|
||||
engine.telemetry.addLine("Servos");
|
||||
engine.telemetry.addData("Gripper", gripper.getPosition());
|
||||
engine.telemetry.addData("Arm", gripper.getPosition());
|
||||
engine.telemetry.addLine();
|
||||
engine.telemetry.addData("Arm Stack Position", armStackPosition);
|
||||
|
||||
engine.telemetry.addLine();
|
||||
}
|
||||
|
||||
public void teleopTelemetry() {
|
||||
engine.telemetry.addLine();
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,185 @@
|
||||
package dev.cyberarm.minibots.pizza.engines;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
import dev.cyberarm.minibots.pizza.PizzaMinibot;
|
||||
import dev.cyberarm.minibots.pizza.states.Rotate;
|
||||
import dev.cyberarm.minibots.pizza.states.ServoMove;
|
||||
import dev.cyberarm.minibots.pizza.states.StrafeMove;
|
||||
import dev.cyberarm.minibots.pizza.states.TankMove;
|
||||
|
||||
@Autonomous(name = "Cyberarm Pizza Autonomous BLUE LEFT", group = "MINIBOT", preselectTeleOp = "Cyberarm Pizza Teleop")
|
||||
public class PizzaAutonomousBlueLeftEngine extends CyberarmEngine {
|
||||
private PizzaMinibot robot;
|
||||
@Override
|
||||
public void setup() {
|
||||
this.robot = new PizzaMinibot(this);
|
||||
robot.imu.resetYaw();
|
||||
|
||||
/// Close the gripper
|
||||
addState(new ServoMove(
|
||||
robot,
|
||||
"gripper",
|
||||
PizzaMinibot.GRIPPER_CLOSED,
|
||||
100
|
||||
));
|
||||
|
||||
/// Move Purple Pixel into position
|
||||
addState(new TankMove(
|
||||
robot,
|
||||
-900,
|
||||
-900,
|
||||
-0.15,
|
||||
-0.17,
|
||||
10,
|
||||
5000
|
||||
));
|
||||
addState(new Rotate(
|
||||
robot,
|
||||
0,
|
||||
0.15,
|
||||
1,
|
||||
5000
|
||||
));
|
||||
|
||||
/// Retreat from pixel
|
||||
addState(new TankMove(
|
||||
robot,
|
||||
150,
|
||||
150,
|
||||
0.15,
|
||||
0.17,
|
||||
10,
|
||||
5000
|
||||
));
|
||||
/// Rotate "towards" backstage board
|
||||
addState(new Rotate(
|
||||
robot,
|
||||
90,
|
||||
0.15,
|
||||
1,
|
||||
5000
|
||||
));
|
||||
|
||||
/// Move clear of the pixel
|
||||
addState(new TankMove(
|
||||
robot,
|
||||
500,
|
||||
500,
|
||||
0.15,
|
||||
0.17,
|
||||
10,
|
||||
5000
|
||||
));
|
||||
/// Strafe so arm is in range of board
|
||||
addState(new StrafeMove(
|
||||
robot,
|
||||
1000,
|
||||
0.5,
|
||||
10,
|
||||
5000
|
||||
));
|
||||
/// Correct robots rotation "towards" backstage board
|
||||
addState(new Rotate(
|
||||
robot,
|
||||
90,
|
||||
0.15,
|
||||
1,
|
||||
5000
|
||||
));
|
||||
|
||||
/// Move up to the backboard and pre-position arm
|
||||
addState(new TankMove(
|
||||
robot,
|
||||
500,
|
||||
500,
|
||||
0.15,
|
||||
0.17,
|
||||
10,
|
||||
5000
|
||||
));
|
||||
addParallelStateToLastState(new ServoMove(
|
||||
robot,
|
||||
"arm",
|
||||
PizzaMinibot.ARM_DELIVER,
|
||||
1000
|
||||
));
|
||||
|
||||
/// Correct rotation; Rotate "towards" backstage board
|
||||
addState(new Rotate(
|
||||
robot,
|
||||
90,
|
||||
0.15,
|
||||
1,
|
||||
5000
|
||||
));
|
||||
/// Move up against the backboard
|
||||
addState(new TankMove(
|
||||
robot,
|
||||
200,
|
||||
200,
|
||||
0.15,
|
||||
0.17,
|
||||
10,
|
||||
1250
|
||||
));
|
||||
/// Correct rotation again
|
||||
addState(new Rotate(
|
||||
robot,
|
||||
90,
|
||||
0.15,
|
||||
1,
|
||||
5000
|
||||
));
|
||||
/// Deposit pixel on backboard
|
||||
addState(new ServoMove(
|
||||
robot,
|
||||
"gripper",
|
||||
PizzaMinibot.GRIPPER_OPEN,
|
||||
1000
|
||||
));
|
||||
/// Stow the arm
|
||||
addState(new ServoMove(
|
||||
robot,
|
||||
"arm",
|
||||
PizzaMinibot.ARM_STOW,
|
||||
250
|
||||
));
|
||||
/// Move away from the backboard
|
||||
addState(new TankMove(
|
||||
robot,
|
||||
-250,
|
||||
-250,
|
||||
-0.15,
|
||||
-0.17,
|
||||
10,
|
||||
5000
|
||||
));
|
||||
/// Rotate towards the corner
|
||||
addState(new Rotate(
|
||||
robot,
|
||||
178,
|
||||
0.15,
|
||||
1,
|
||||
5000
|
||||
));
|
||||
/// Move into corner
|
||||
addState(new TankMove(
|
||||
robot,
|
||||
-1000,
|
||||
-1000,
|
||||
-0.15,
|
||||
-0.17,
|
||||
10,
|
||||
5000
|
||||
));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
robot.standardTelemetry();
|
||||
|
||||
super.loop();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,157 @@
|
||||
package dev.cyberarm.minibots.pizza.engines;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
import dev.cyberarm.minibots.pizza.PizzaMinibot;
|
||||
import dev.cyberarm.minibots.pizza.states.Rotate;
|
||||
import dev.cyberarm.minibots.pizza.states.ServoMove;
|
||||
import dev.cyberarm.minibots.pizza.states.TankMove;
|
||||
|
||||
@Autonomous(name = "Cyberarm Pizza Autonomous RED RIGHT", group = "MINIBOT", preselectTeleOp = "Cyberarm Pizza Teleop")
|
||||
public class PizzaAutonomousRedRightEngine extends CyberarmEngine {
|
||||
private PizzaMinibot robot;
|
||||
@Override
|
||||
public void setup() {
|
||||
this.robot = new PizzaMinibot(this);
|
||||
robot.imu.resetYaw();
|
||||
|
||||
/// Close the gripper
|
||||
addState(new ServoMove(
|
||||
robot,
|
||||
"gripper",
|
||||
PizzaMinibot.GRIPPER_CLOSED,
|
||||
100
|
||||
));
|
||||
|
||||
/// Move Purple Pixel into position
|
||||
addState(new TankMove(
|
||||
robot,
|
||||
-900,
|
||||
-900,
|
||||
-0.15,
|
||||
-0.17,
|
||||
10,
|
||||
5000
|
||||
));
|
||||
addState(new Rotate(
|
||||
robot,
|
||||
0,
|
||||
0.15,
|
||||
1,
|
||||
5000
|
||||
));
|
||||
|
||||
/// Retreat from pixel
|
||||
addState(new TankMove(
|
||||
robot,
|
||||
150,
|
||||
150,
|
||||
0.15,
|
||||
0.17,
|
||||
10,
|
||||
5000
|
||||
));
|
||||
/// Rotate "towards" backstage board
|
||||
addState(new Rotate(
|
||||
robot,
|
||||
270,
|
||||
0.15,
|
||||
1,
|
||||
5000
|
||||
));
|
||||
|
||||
/// Move up to the backboard and pre-position arm
|
||||
addState(new TankMove(
|
||||
robot,
|
||||
900,
|
||||
900,
|
||||
0.15,
|
||||
0.17,
|
||||
10,
|
||||
5000
|
||||
));
|
||||
addParallelStateToLastState(new ServoMove(
|
||||
robot,
|
||||
"arm",
|
||||
PizzaMinibot.ARM_DELIVER,
|
||||
1000
|
||||
));
|
||||
|
||||
/// Correct rotation; Rotate "towards" backstage board
|
||||
addState(new Rotate(
|
||||
robot,
|
||||
270,
|
||||
0.15,
|
||||
1,
|
||||
5000
|
||||
));
|
||||
/// Move up against the backboard
|
||||
addState(new TankMove(
|
||||
robot,
|
||||
200,
|
||||
200,
|
||||
0.15,
|
||||
0.17,
|
||||
10,
|
||||
5000
|
||||
));
|
||||
/// Correct rotation again
|
||||
addState(new Rotate(
|
||||
robot,
|
||||
270,
|
||||
0.15,
|
||||
1,
|
||||
5000
|
||||
));
|
||||
/// Deposit pixel on backboard
|
||||
addState(new ServoMove(
|
||||
robot,
|
||||
"gripper",
|
||||
PizzaMinibot.GRIPPER_OPEN,
|
||||
1000
|
||||
));
|
||||
/// Stow the arm
|
||||
addState(new ServoMove(
|
||||
robot,
|
||||
"arm",
|
||||
PizzaMinibot.ARM_STOW,
|
||||
250
|
||||
));
|
||||
/// Move away from the backboard
|
||||
addState(new TankMove(
|
||||
robot,
|
||||
-250,
|
||||
-250,
|
||||
-0.15,
|
||||
-0.17,
|
||||
10,
|
||||
5000
|
||||
));
|
||||
/// Rotate towards the corner
|
||||
addState(new Rotate(
|
||||
robot,
|
||||
170,
|
||||
0.15,
|
||||
1,
|
||||
5000
|
||||
));
|
||||
/// Move into corner
|
||||
addState(new TankMove(
|
||||
robot,
|
||||
-500,
|
||||
-500,
|
||||
-0.15,
|
||||
-0.17,
|
||||
10,
|
||||
5000
|
||||
));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
robot.standardTelemetry();
|
||||
|
||||
super.loop();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
package dev.cyberarm.minibots.pizza.engines;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
import dev.cyberarm.minibots.pizza.PizzaMinibot;
|
||||
import dev.cyberarm.minibots.pizza.states.Pilot;
|
||||
|
||||
@TeleOp(name = "Cyberarm Pizza Teleop", group = "MINIBOT")
|
||||
public class PizzaTeleOpEngine extends CyberarmEngine {
|
||||
PizzaMinibot robot;
|
||||
@Override
|
||||
public void setup() {
|
||||
robot = new PizzaMinibot(this);
|
||||
|
||||
addState(new Pilot(robot));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
robot.standardTelemetry();
|
||||
robot.teleopTelemetry();
|
||||
|
||||
super.loop();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,148 @@
|
||||
package dev.cyberarm.minibots.pizza.states;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
import dev.cyberarm.minibots.pizza.PizzaMinibot;
|
||||
|
||||
public class Pilot extends CyberarmState {
|
||||
final PizzaMinibot robot;
|
||||
|
||||
public Pilot(PizzaMinibot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
@Override
|
||||
public void exec() {
|
||||
/// --- IMU Reset --- ///
|
||||
if (engine.gamepad1.guide) {
|
||||
robot.imu.resetYaw();
|
||||
}
|
||||
|
||||
/// --- GRIPPER --- ///
|
||||
if (engine.gamepad1.x) {
|
||||
robot.gripper.setPosition(PizzaMinibot.GRIPPER_CLOSED);
|
||||
} else if (engine.gamepad1.b) {
|
||||
robot.gripper.setPosition(PizzaMinibot.GRIPPER_OPEN);
|
||||
}
|
||||
|
||||
/// --- ARM --- ///
|
||||
if (engine.gamepad1.dpad_down) {
|
||||
robot.arm.setPosition(PizzaMinibot.ARM_STOW);
|
||||
robot.armStackPosition = -1;
|
||||
} else if (engine.gamepad1.dpad_up) {
|
||||
robot.arm.setPosition(PizzaMinibot.ARM_COLLECT);
|
||||
robot.armStackPosition = -1;
|
||||
} else if (engine.gamepad1.dpad_right) {
|
||||
robot.arm.setPosition(PizzaMinibot.ARM_PRECOLLECT);
|
||||
robot.armStackPosition = -1;
|
||||
} else if (engine.gamepad1.dpad_left) {
|
||||
robot.arm.setPosition(PizzaMinibot.ARM_DELIVER);
|
||||
robot.armStackPosition = -1;
|
||||
}
|
||||
|
||||
/// --- DRIVE --- ///
|
||||
// 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 botHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
|
||||
|
||||
// Rotate the movement direction counter to the bot's rotation
|
||||
double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading);
|
||||
double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading);
|
||||
|
||||
rotX = rotX * 1.1; // Counteract imperfect strafing
|
||||
|
||||
// Denominator is the largest motor power (absolute value) or 1
|
||||
// This ensures all the powers maintain the same ratio,
|
||||
// but only if at least one is out of the range [-1, 1]
|
||||
double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1);
|
||||
double frontLeftPower = (rotY + rotX + rx) / denominator;
|
||||
double backLeftPower = (rotY - rotX + rx) / denominator;
|
||||
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 maxPower = 0.5;
|
||||
|
||||
robot.leftFront.motorEx.setPower(frontLeftPower * maxPower);
|
||||
robot.leftBack.motorEx.setPower(backLeftPower * maxPower);
|
||||
robot.rightFront.motorEx.setPower(frontRightPower * maxPower);
|
||||
robot.rightBack.motorEx.setPower(backRightPower * maxPower);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void buttonDown(Gamepad gamepad, String button) {
|
||||
if (gamepad == engine.gamepad1) {
|
||||
if (button.equals("left_bumper")) {
|
||||
switch (robot.armStackPosition) {
|
||||
case -1:
|
||||
robot.arm.setPosition(PizzaMinibot.ARM_HOVER_5_STACK);
|
||||
robot.armStackPosition = 5;
|
||||
break;
|
||||
case 5:
|
||||
robot.arm.setPosition(PizzaMinibot.ARM_HOVER_5_STACK);
|
||||
robot.armStackPosition = 5;
|
||||
break;
|
||||
case 4:
|
||||
robot.arm.setPosition(PizzaMinibot.ARM_HOVER_5_STACK);
|
||||
robot.armStackPosition = 5;
|
||||
break;
|
||||
case 3:
|
||||
robot.arm.setPosition(PizzaMinibot.ARM_HOVER_4_STACK);
|
||||
robot.armStackPosition = 4;
|
||||
break;
|
||||
case 2:
|
||||
robot.arm.setPosition(PizzaMinibot.ARM_HOVER_3_STACK);
|
||||
robot.armStackPosition = 3;
|
||||
break;
|
||||
case 1:
|
||||
robot.arm.setPosition(PizzaMinibot.ARM_HOVER_2_STACK);
|
||||
robot.armStackPosition = 2;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (button.equals("right_bumper")) {
|
||||
switch (robot.armStackPosition) {
|
||||
case -1:
|
||||
robot.arm.setPosition(PizzaMinibot.ARM_HOVER_5_STACK);
|
||||
robot.armStackPosition = 5;
|
||||
break;
|
||||
case 5:
|
||||
robot.arm.setPosition(PizzaMinibot.ARM_HOVER_4_STACK);
|
||||
robot.armStackPosition = 4;
|
||||
break;
|
||||
case 4:
|
||||
robot.arm.setPosition(PizzaMinibot.ARM_HOVER_3_STACK);
|
||||
robot.armStackPosition = 3;
|
||||
break;
|
||||
case 3:
|
||||
robot.arm.setPosition(PizzaMinibot.ARM_HOVER_2_STACK);
|
||||
robot.armStackPosition = 2;
|
||||
break;
|
||||
case 2:
|
||||
robot.arm.setPosition(PizzaMinibot.ARM_HOVER_1_STACK);
|
||||
robot.armStackPosition = 1;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,80 @@
|
||||
package dev.cyberarm.minibots.pizza.states;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
import dev.cyberarm.minibots.pizza.PizzaMinibot;
|
||||
|
||||
public class Rotate extends CyberarmState {
|
||||
final private PizzaMinibot robot;
|
||||
final private String groupName, actionName;
|
||||
|
||||
final private double heading, velocity, tolerance;
|
||||
final private int timeoutMS;
|
||||
|
||||
public Rotate(PizzaMinibot robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
|
||||
this.groupName = groupName;
|
||||
this.actionName = actionName;
|
||||
|
||||
this.heading = robot.config.variable(groupName, actionName, "heading").value();
|
||||
|
||||
this.velocity = robot.config.variable(groupName, actionName, "velocity").value();
|
||||
|
||||
this.tolerance = robot.config.variable(groupName, actionName, "tolerance").value();
|
||||
|
||||
this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value();
|
||||
}
|
||||
|
||||
public Rotate(PizzaMinibot robot, double heading, double velocity, double tolerance, int timeoutMS) {
|
||||
this.groupName = "";
|
||||
this.actionName = "";
|
||||
|
||||
this.robot = robot;
|
||||
this.heading = heading;
|
||||
this.velocity = velocity;
|
||||
this.tolerance = tolerance;
|
||||
this.timeoutMS = timeoutMS;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
}
|
||||
|
||||
@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);
|
||||
}
|
||||
|
||||
if (robot.angleDiff(robot.facing() + robot.turnRate(), heading) < 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());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,55 @@
|
||||
package dev.cyberarm.minibots.pizza.states;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
import dev.cyberarm.minibots.pizza.PizzaMinibot;
|
||||
|
||||
public class ServoMove extends CyberarmState {
|
||||
final private PizzaMinibot robot;
|
||||
final private String groupName, actionName;
|
||||
|
||||
final private String servoName;
|
||||
final private double position;
|
||||
final private int timeoutMS;
|
||||
|
||||
final private Servo servo;
|
||||
public ServoMove(PizzaMinibot robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
|
||||
this.groupName = groupName;
|
||||
this.actionName = actionName;
|
||||
|
||||
this.servoName = robot.config.variable(groupName, actionName, "servoName").value();
|
||||
|
||||
this.position = robot.config.variable(groupName, actionName, "position").value();
|
||||
|
||||
this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value();
|
||||
|
||||
this.servo = engine.hardwareMap.servo.get(servoName);
|
||||
}
|
||||
|
||||
public ServoMove(PizzaMinibot robot, String servoName, double position, int timeoutMS) {
|
||||
this.groupName = "";
|
||||
this.actionName = "";
|
||||
|
||||
this.robot = robot;
|
||||
this.servoName = servoName;
|
||||
this.position = position;
|
||||
this.timeoutMS = timeoutMS;
|
||||
|
||||
this.servo = engine.hardwareMap.servo.get(servoName);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
this.servo.setPosition(this.position);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
if (runTime() >= timeoutMS) {
|
||||
setHasFinished(true);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,62 @@
|
||||
package dev.cyberarm.minibots.pizza.states;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
import dev.cyberarm.minibots.pizza.PizzaMinibot;
|
||||
|
||||
public class StrafeMove extends CyberarmState {
|
||||
final private PizzaMinibot robot;
|
||||
final private String groupName, actionName;
|
||||
|
||||
final private double distanceMM, velocity;
|
||||
final private int tolerance, timeoutMS;
|
||||
|
||||
public StrafeMove(PizzaMinibot robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
|
||||
this.groupName = groupName;
|
||||
this.actionName = actionName;
|
||||
|
||||
this.distanceMM = robot.config.variable(groupName, actionName, "distanceMM").value();
|
||||
|
||||
this.velocity = robot.config.variable(groupName, actionName, "velocity").value();
|
||||
|
||||
this.tolerance = robot.config.variable(groupName, actionName, "tolerance").value();
|
||||
this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value();
|
||||
}
|
||||
|
||||
public StrafeMove(PizzaMinibot robot, double distanceMM, double velocity, int tolerance, int timeoutMS) {
|
||||
this.groupName = "";
|
||||
this.actionName = "";
|
||||
|
||||
this.robot = robot;
|
||||
this.distanceMM = distanceMM;
|
||||
this.velocity = velocity;
|
||||
this.tolerance = tolerance;
|
||||
this.timeoutMS = timeoutMS;
|
||||
}
|
||||
|
||||
@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);
|
||||
}
|
||||
|
||||
@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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,92 @@
|
||||
package dev.cyberarm.minibots.pizza.states;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
import dev.cyberarm.minibots.pizza.PizzaMinibot;
|
||||
|
||||
public class TankMove extends CyberarmState {
|
||||
final private PizzaMinibot robot;
|
||||
final private String groupName, actionName;
|
||||
|
||||
final private double leftDistanceMM, rightDistanceMM;
|
||||
final private double leftVelocity, rightVelocity;
|
||||
final private int tolerance, timeoutMS;
|
||||
|
||||
public TankMove(PizzaMinibot robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
|
||||
this.groupName = groupName;
|
||||
this.actionName = actionName;
|
||||
|
||||
this.leftDistanceMM = robot.config.variable(groupName, actionName, "leftDistanceMM").value();
|
||||
this.rightDistanceMM = robot.config.variable(groupName, actionName, "rightDistanceMM").value();
|
||||
|
||||
this.leftVelocity = robot.config.variable(groupName, actionName, "leftVelocity").value();
|
||||
this.rightVelocity = robot.config.variable(groupName, actionName, "rightVelocity").value();
|
||||
|
||||
this.tolerance = robot.config.variable(groupName, actionName, "tolerance").value();
|
||||
this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value();
|
||||
}
|
||||
|
||||
public TankMove(PizzaMinibot robot, double leftDistanceMM, double rightDistanceMM, double leftVelocity, double rightVelocity, int tolerance, int timeoutMS) {
|
||||
this.groupName = "";
|
||||
this.actionName = "";
|
||||
|
||||
this.robot = robot;
|
||||
this.leftDistanceMM = leftDistanceMM;
|
||||
this.rightDistanceMM = rightDistanceMM;
|
||||
this.leftVelocity = leftVelocity;
|
||||
this.rightVelocity = rightVelocity;
|
||||
this.tolerance = tolerance;
|
||||
this.timeoutMS = timeoutMS;
|
||||
}
|
||||
|
||||
@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);
|
||||
}
|
||||
|
||||
@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);
|
||||
}
|
||||
}
|
||||
|
||||
@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());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,142 @@
|
||||
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.robotcore.hardware.CRServo;
|
||||
import com.qualcomm.robotcore.hardware.IMU;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
|
||||
|
||||
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 YellowMinibot(CyberarmEngine engine) {
|
||||
this.engine = engine;
|
||||
|
||||
this.config = new TimeCraftersConfiguration("Vexy_2023");
|
||||
|
||||
leftFront = new MotorEx(engine.hardwareMap, "leftFront");
|
||||
rightFront = new MotorEx(engine.hardwareMap, "rightFront");
|
||||
|
||||
leftBack = new MotorEx(engine.hardwareMap, "leftBack");
|
||||
rightBack = new MotorEx(engine.hardwareMap, "rightBack");
|
||||
|
||||
wheelCircumference = Math.PI * (50.0 * 2); //wheelRadius * wheelRadius;
|
||||
distancePerTick = 288 / wheelCircumference; // raw motor encoder * gear ratio
|
||||
|
||||
leftFront.setDistancePerPulse(distancePerTick);
|
||||
rightFront.setDistancePerPulse(distancePerTick);
|
||||
leftBack.setDistancePerPulse(distancePerTick);
|
||||
rightBack.setDistancePerPulse(distancePerTick);
|
||||
|
||||
left = new MotorGroup(leftFront, leftBack);
|
||||
right = new MotorGroup(rightFront, rightBack);
|
||||
|
||||
imu = engine.hardwareMap.get(IMU.class, "imu");
|
||||
IMU.Parameters parameters = new IMU.Parameters(
|
||||
new RevHubOrientationOnRobot(
|
||||
RevHubOrientationOnRobot.LogoFacingDirection.UP,
|
||||
RevHubOrientationOnRobot.UsbFacingDirection.RIGHT));
|
||||
|
||||
imu.initialize(parameters);
|
||||
|
||||
imuAngleOffset = 0;
|
||||
initialFacing = facing();
|
||||
|
||||
droneLauncher = engine.hardwareMap.crservo.get("droneLauncher"); /// Port 5
|
||||
}
|
||||
|
||||
public void standardTelemetry() {
|
||||
engine.telemetry.addLine();
|
||||
|
||||
engine.telemetry.addLine("Motors");
|
||||
engine.telemetry.addData(
|
||||
"Left Front",
|
||||
"Power: %.2f, Current: %.2f mAmp, Position: %d",
|
||||
leftFront.motorEx.getPower(),
|
||||
leftFront.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
|
||||
leftFront.motorEx.getCurrentPosition());
|
||||
engine.telemetry.addData(
|
||||
"Right Front",
|
||||
"Power: %.2f, Current: %.2f mAmp, Position: %d",
|
||||
rightFront.motorEx.getPower(),
|
||||
rightFront.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
|
||||
rightFront.motorEx.getCurrentPosition());
|
||||
engine.telemetry.addData(
|
||||
"Left Back",
|
||||
"Power: %.2f, Current: %.2f mAmp, Position: %d",
|
||||
leftBack.motorEx.getPower(),
|
||||
leftBack.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
|
||||
leftBack.motorEx.getCurrentPosition());
|
||||
engine.telemetry.addData(
|
||||
"Right Back",
|
||||
"Power: %.2f, Current: %.2f mAmp, Position: %d",
|
||||
rightBack.motorEx.getPower(),
|
||||
rightBack.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
|
||||
rightBack.motorEx.getCurrentPosition());
|
||||
|
||||
engine.telemetry.addLine();
|
||||
engine.telemetry.addLine("Servos");
|
||||
engine.telemetry.addData("droneLauncher", droneLauncher.getPower());
|
||||
|
||||
engine.telemetry.addLine();
|
||||
}
|
||||
|
||||
public void teleopTelemetry() {
|
||||
engine.telemetry.addLine();
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,78 @@
|
||||
package dev.cyberarm.minibots.yellow.engines;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
import dev.cyberarm.minibots.yellow.YellowMinibot;
|
||||
import dev.cyberarm.minibots.yellow.states.Rotate;
|
||||
import dev.cyberarm.minibots.yellow.states.TankMove;
|
||||
|
||||
@Autonomous(name = "Cyberarm Yellow Autonomous BLUE RIGHT", group = "MINIBOT", preselectTeleOp = "Cyberarm Yellow Teleop")
|
||||
public class YellowMinibotAutonomousBlueRightEngine extends CyberarmEngine {
|
||||
@Override
|
||||
public void setup() {
|
||||
YellowMinibot robot = new YellowMinibot(this);
|
||||
robot.imu.resetYaw();
|
||||
|
||||
/// Move Purple pixel into position
|
||||
addState(new TankMove(
|
||||
robot,
|
||||
1450,
|
||||
1450,
|
||||
-1.0,
|
||||
-1.0,
|
||||
10,
|
||||
5000
|
||||
));
|
||||
/// Move away from pixel
|
||||
addState(new TankMove(
|
||||
robot,
|
||||
250,
|
||||
250,
|
||||
1.0,
|
||||
1.0,
|
||||
10,
|
||||
5000
|
||||
));
|
||||
/// Turn around
|
||||
addState(new Rotate(
|
||||
robot,
|
||||
182,
|
||||
1.0,
|
||||
0.25,
|
||||
1,
|
||||
5000
|
||||
));
|
||||
/// Move back towards yellow pixel
|
||||
addState(new TankMove(
|
||||
robot,
|
||||
850,
|
||||
850,
|
||||
-1.0,
|
||||
-1.0,
|
||||
10,
|
||||
5000
|
||||
));
|
||||
/// Turn around
|
||||
addState(new Rotate(
|
||||
robot,
|
||||
270,
|
||||
1.0,
|
||||
0.25,
|
||||
1,
|
||||
5000
|
||||
));
|
||||
/// Move towards backstage
|
||||
addState(new TankMove(
|
||||
robot,
|
||||
4000,
|
||||
4000,
|
||||
-1.0,
|
||||
-1.0,
|
||||
10,
|
||||
8000
|
||||
));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,76 @@
|
||||
package dev.cyberarm.minibots.yellow.engines;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
import dev.cyberarm.minibots.yellow.YellowMinibot;
|
||||
import dev.cyberarm.minibots.yellow.states.Rotate;
|
||||
import dev.cyberarm.minibots.yellow.states.TankMove;
|
||||
|
||||
@Autonomous(name = "Cyberarm Yellow Autonomous RED LEFT", group = "MINIBOT", preselectTeleOp = "Cyberarm Yellow Teleop")
|
||||
public class YellowMinibotAutonomousRedLeftEngine extends CyberarmEngine {
|
||||
@Override
|
||||
public void setup() {
|
||||
YellowMinibot robot = new YellowMinibot(this);
|
||||
robot.imu.resetYaw();
|
||||
|
||||
/// Move Purple pixel into position
|
||||
addState(new TankMove(
|
||||
robot,
|
||||
1450,
|
||||
1450,
|
||||
-1.0,
|
||||
-1.0,
|
||||
10,
|
||||
5000
|
||||
));
|
||||
/// Move away from pixel
|
||||
addState(new TankMove(
|
||||
robot,
|
||||
250,
|
||||
250,
|
||||
1.0,
|
||||
1.0,
|
||||
10,
|
||||
5000
|
||||
));
|
||||
/// Turn around
|
||||
addState(new Rotate(
|
||||
robot,
|
||||
178,
|
||||
1.0,
|
||||
0.25,
|
||||
1,
|
||||
5000
|
||||
));
|
||||
/// Move back towards yellow pixel
|
||||
addState(new TankMove(
|
||||
robot,
|
||||
850,
|
||||
850,
|
||||
-1.0,
|
||||
-1.0,
|
||||
10,
|
||||
5000
|
||||
));
|
||||
/// Turn around
|
||||
addState(new Rotate(
|
||||
robot,
|
||||
90,
|
||||
1.0,
|
||||
0.25,
|
||||
1,
|
||||
5000
|
||||
));
|
||||
/// Move towards backstage
|
||||
addState(new TankMove(
|
||||
robot,
|
||||
4000,
|
||||
4000,
|
||||
-1.0,
|
||||
-1.0,
|
||||
10,
|
||||
8000
|
||||
));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
package dev.cyberarm.minibots.yellow.engines;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
import dev.cyberarm.minibots.yellow.YellowMinibot;
|
||||
import dev.cyberarm.minibots.yellow.states.Pilot;
|
||||
|
||||
@TeleOp(name = "Cyberarm Yellow Teleop", group = "MINIBOT")
|
||||
public class YellowMinibotTeleOpEngine extends CyberarmEngine {
|
||||
public YellowMinibot robot;
|
||||
@Override
|
||||
public void setup() {
|
||||
robot = new YellowMinibot(this);
|
||||
|
||||
addState(new Pilot(robot));
|
||||
// addState(new Move(robot, 10, 8, 0.25, 0.25));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,68 @@
|
||||
package dev.cyberarm.minibots.yellow.states;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
import dev.cyberarm.minibots.yellow.YellowMinibot;
|
||||
|
||||
public class Pilot extends CyberarmState {
|
||||
final YellowMinibot robot;
|
||||
|
||||
public Pilot(YellowMinibot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
@Override
|
||||
public void exec() {
|
||||
/// --- IMU Reset --- ///
|
||||
if (engine.gamepad1.guide) {
|
||||
robot.imu.resetYaw();
|
||||
}
|
||||
|
||||
/// --- DRONE --- ///
|
||||
if (engine.gamepad1.y) {
|
||||
robot.droneLauncher.setPower(1.0);
|
||||
} else if (engine.gamepad1.a) {
|
||||
robot.droneLauncher.setPower(0.0);
|
||||
}
|
||||
|
||||
/// --- DRIVE --- ///
|
||||
// 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 botHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
|
||||
|
||||
// Rotate the movement direction counter to the bot's rotation
|
||||
double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading);
|
||||
double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading);
|
||||
|
||||
rotX = rotX * 1.1; // Counteract imperfect strafing
|
||||
|
||||
// Denominator is the largest motor power (absolute value) or 1
|
||||
// This ensures all the powers maintain the same ratio,
|
||||
// but only if at least one is out of the range [-1, 1]
|
||||
double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1);
|
||||
double frontLeftPower = (rotY + rotX + rx) / denominator;
|
||||
double backLeftPower = (rotY - rotX + rx) / denominator;
|
||||
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 maxPower = 1.0;
|
||||
|
||||
robot.leftFront.motorEx.setPower(frontLeftPower * maxPower);
|
||||
robot.leftBack.motorEx.setPower(backLeftPower * maxPower);
|
||||
robot.rightFront.motorEx.setPower(frontRightPower * maxPower);
|
||||
robot.rightBack.motorEx.setPower(backRightPower * maxPower);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,86 @@
|
||||
package dev.cyberarm.minibots.yellow.states;
|
||||
|
||||
import com.qualcomm.robotcore.util.Range;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
import dev.cyberarm.minibots.pizza.PizzaMinibot;
|
||||
import dev.cyberarm.minibots.yellow.YellowMinibot;
|
||||
|
||||
public class Rotate extends CyberarmState {
|
||||
final private YellowMinibot robot;
|
||||
final private String groupName, actionName;
|
||||
|
||||
final private double heading, maxVelocity, minVelocity, tolerance;
|
||||
final private int timeoutMS;
|
||||
|
||||
public Rotate(YellowMinibot robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
|
||||
this.groupName = groupName;
|
||||
this.actionName = actionName;
|
||||
|
||||
this.heading = robot.config.variable(groupName, actionName, "heading").value();
|
||||
|
||||
this.minVelocity = robot.config.variable(groupName, actionName, "minVelocity").value();
|
||||
this.maxVelocity = robot.config.variable(groupName, actionName, "maxVelocity").value();
|
||||
|
||||
this.tolerance = robot.config.variable(groupName, actionName, "tolerance").value();
|
||||
|
||||
this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value();
|
||||
}
|
||||
|
||||
public Rotate(YellowMinibot robot, double heading, double maxVelocity, double minVelocity, double tolerance, int timeoutMS) {
|
||||
this.groupName = "";
|
||||
this.actionName = "";
|
||||
|
||||
this.robot = robot;
|
||||
this.heading = heading;
|
||||
this.maxVelocity = maxVelocity;
|
||||
this.minVelocity = minVelocity;
|
||||
this.tolerance = tolerance;
|
||||
this.timeoutMS = timeoutMS;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
}
|
||||
|
||||
@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);
|
||||
}
|
||||
}
|
||||
|
||||
@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());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
package dev.cyberarm.minibots.yellow.states;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
import dev.cyberarm.minibots.pizza.PizzaMinibot;
|
||||
import dev.cyberarm.minibots.yellow.YellowMinibot;
|
||||
|
||||
public class ServoMove extends CyberarmState {
|
||||
final private YellowMinibot robot;
|
||||
final private String groupName, actionName;
|
||||
|
||||
final private String servoName;
|
||||
final private double position;
|
||||
final private int timeoutMS;
|
||||
|
||||
final private Servo servo;
|
||||
public ServoMove(YellowMinibot robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
|
||||
this.groupName = groupName;
|
||||
this.actionName = actionName;
|
||||
|
||||
this.servoName = robot.config.variable(groupName, actionName, "servoName").value();
|
||||
|
||||
this.position = robot.config.variable(groupName, actionName, "position").value();
|
||||
|
||||
this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value();
|
||||
|
||||
this.servo = engine.hardwareMap.servo.get(servoName);
|
||||
}
|
||||
|
||||
public ServoMove(YellowMinibot robot, String servoName, double position, int timeoutMS) {
|
||||
this.groupName = "";
|
||||
this.actionName = "";
|
||||
|
||||
this.robot = robot;
|
||||
this.servoName = servoName;
|
||||
this.position = position;
|
||||
this.timeoutMS = timeoutMS;
|
||||
|
||||
this.servo = engine.hardwareMap.servo.get(servoName);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
this.servo.setPosition(this.position);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
if (runTime() >= timeoutMS) {
|
||||
setHasFinished(true);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,63 @@
|
||||
package dev.cyberarm.minibots.yellow.states;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
import dev.cyberarm.minibots.pizza.PizzaMinibot;
|
||||
import dev.cyberarm.minibots.yellow.YellowMinibot;
|
||||
|
||||
public class StrafeMove extends CyberarmState {
|
||||
final private YellowMinibot robot;
|
||||
final private String groupName, actionName;
|
||||
|
||||
final private double distanceMM, velocity;
|
||||
final private int tolerance, timeoutMS;
|
||||
|
||||
public StrafeMove(YellowMinibot robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
|
||||
this.groupName = groupName;
|
||||
this.actionName = actionName;
|
||||
|
||||
this.distanceMM = robot.config.variable(groupName, actionName, "distanceMM").value();
|
||||
|
||||
this.velocity = robot.config.variable(groupName, actionName, "velocity").value();
|
||||
|
||||
this.tolerance = robot.config.variable(groupName, actionName, "tolerance").value();
|
||||
this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value();
|
||||
}
|
||||
|
||||
public StrafeMove(YellowMinibot robot, double distanceMM, double velocity, int tolerance, int timeoutMS) {
|
||||
this.groupName = "";
|
||||
this.actionName = "";
|
||||
|
||||
this.robot = robot;
|
||||
this.distanceMM = distanceMM;
|
||||
this.velocity = velocity;
|
||||
this.tolerance = tolerance;
|
||||
this.timeoutMS = timeoutMS;
|
||||
}
|
||||
|
||||
@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);
|
||||
}
|
||||
|
||||
@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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,93 @@
|
||||
package dev.cyberarm.minibots.yellow.states;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
import dev.cyberarm.minibots.pizza.PizzaMinibot;
|
||||
import dev.cyberarm.minibots.yellow.YellowMinibot;
|
||||
|
||||
public class TankMove extends CyberarmState {
|
||||
final private YellowMinibot robot;
|
||||
final private String groupName, actionName;
|
||||
|
||||
final private double leftDistanceMM, rightDistanceMM;
|
||||
final private double leftVelocity, rightVelocity;
|
||||
final private int tolerance, timeoutMS;
|
||||
|
||||
public TankMove(YellowMinibot robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
|
||||
this.groupName = groupName;
|
||||
this.actionName = actionName;
|
||||
|
||||
this.leftDistanceMM = robot.config.variable(groupName, actionName, "leftDistanceMM").value();
|
||||
this.rightDistanceMM = robot.config.variable(groupName, actionName, "rightDistanceMM").value();
|
||||
|
||||
this.leftVelocity = robot.config.variable(groupName, actionName, "leftVelocity").value();
|
||||
this.rightVelocity = robot.config.variable(groupName, actionName, "rightVelocity").value();
|
||||
|
||||
this.tolerance = robot.config.variable(groupName, actionName, "tolerance").value();
|
||||
this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value();
|
||||
}
|
||||
|
||||
public TankMove(YellowMinibot robot, double leftDistanceMM, double rightDistanceMM, double leftVelocity, double rightVelocity, int tolerance, int timeoutMS) {
|
||||
this.groupName = "";
|
||||
this.actionName = "";
|
||||
|
||||
this.robot = robot;
|
||||
this.leftDistanceMM = leftDistanceMM;
|
||||
this.rightDistanceMM = rightDistanceMM;
|
||||
this.leftVelocity = leftVelocity;
|
||||
this.rightVelocity = rightVelocity;
|
||||
this.tolerance = tolerance;
|
||||
this.timeoutMS = timeoutMS;
|
||||
}
|
||||
|
||||
@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);
|
||||
}
|
||||
|
||||
@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);
|
||||
}
|
||||
}
|
||||
|
||||
@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());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,10 @@
|
||||
package org.timecrafters.CenterStage.Autonomous.Engines;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
|
||||
public class SodiPizzaAutoRedRightEngine extends CyberarmEngine {
|
||||
@Override
|
||||
public void setup() {
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
package org.timecrafters.CenterStage.Common;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
import com.qualcomm.robotcore.hardware.IMU;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import org.timecrafters.Library.Robot;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
|
||||
public class SodiPizzaMinibotObject extends Robot {
|
||||
|
||||
public HardwareMap hardwareMap;
|
||||
public DcMotor flDrive, frDrive, blDrive, brDrive;
|
||||
public Servo shoulder, hand;
|
||||
public IMU imu;
|
||||
private String string;
|
||||
|
||||
private CyberarmEngine engine;
|
||||
public TimeCraftersConfiguration configuration;
|
||||
|
||||
public SodiPizzaMinibotObject() {}
|
||||
|
||||
@Override
|
||||
public void setup() {
|
||||
|
||||
this.engine = CyberarmEngine.instance;
|
||||
this.hardwareMap = CyberarmEngine.instance.hardwareMap;
|
||||
|
||||
//Motor defining
|
||||
flDrive = engine.hardwareMap.dcMotor.get("FL Drive");
|
||||
frDrive = engine.hardwareMap.dcMotor.get("FR Drive");
|
||||
blDrive = engine.hardwareMap.dcMotor.get("BL Drive");
|
||||
brDrive = engine.hardwareMap.dcMotor.get("BR Drive");
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
@@ -19,7 +19,7 @@ public class MiniYellowTeleOPv2 extends CyberarmState {
|
||||
public MotorEx flDrive, frDrive, blDrive, brDrive;
|
||||
public IMU imu;
|
||||
private double flPower, frPower, blPower, brPower;
|
||||
private float lStickY, yTransitPercent, xTransitPercent, rotPercent, percentDenom;
|
||||
private float yTransitPercent, xTransitPercent, rotPercent, percentDenom;
|
||||
|
||||
public TimeCraftersConfiguration configuration;
|
||||
@Override
|
||||
|
||||
Reference in New Issue
Block a user