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

Untested RedCrab minibot controller

Browse Source
This commit is contained in:
Cyberarm
2023-12-04 13:52:46 -06:00
parent 84fd173082
commit f3f3d5408e
4 changed files with 480 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

86
TeamCode/src/main/java/dev/cyberarm/engine/V2/Utilities.java Normal file
View File

@@ -0,0 +1,86 @@
package dev.cyberarm.engine.V2;
import com.qualcomm.robotcore.hardware.IMU;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
public class Utilities {
/***
* The current heading of the robot as a full 360 degree value, instead of that half radian mess.
* @param imu IMU of rev hub or similar
* @param imuAngleOffset optional angle offset added to IMU value
* @return full range heading of the robot, in DEGREES.
*/
static public double facing(IMU imu, double imuAngleOffset) {
double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
return (((imuDegrees + 360.0) % 360.0) + imuAngleOffset) % 360.0;
}
static public double facing(IMU imu) {
return facing(imu, 0);
}
static public double heading(double facing) {
return AngleUnit.normalizeRadians(-facing * Math.PI / 180.0);
}
static public double turnRate(IMU imu) {
return imu.getRobotAngularVelocity(AngleUnit.DEGREES).yRotationRate;
}
/***
*
* @param value value to check with
* @param min minimum value
* @param max maximum value
* @return true if value is between min and max. inclusive.
*/
static public boolean isBetween(double value, double min, double max) {
return value >= min && value <= max;
}
static public boolean isBetween(int value, int min, int max) {
return value >= min && value <= max;
}
// Adapted from: https://github.com/gosu/gosu/blob/980d64de2ce52e4b16fdd5cb9c9e11c8bbb80671/src/Math.cpp#L38
/***
* The angular difference between two angles
* **NOTE** flip flopped from and to values may result in continuous inversion of the angle difference (180 to -180 for example)
* @param from angle in DEGREES
* @param to angle in DEGREES
* @return Angular difference two angles in DEGREES
*/
static 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;
}
/***
* Linear interpolation
* @param min minimum value
* @param max maximum value
* @param t factor
* @return
*/
static public double lerp(double min, double max, double t)
{
return min + (max - min) * t;
}
/***
* Calculates motor angle in ticks
* @param motorTicksPerRevolution
* @param gearRatio
* @param angleInDegrees
* @return Angle in motor ticks
*/
static public int motorAngle(int motorTicksPerRevolution, double gearRatio, double angleInDegrees) {
return (int) (angleInDegrees / (360.0 / (motorTicksPerRevolution * gearRatio)));
}
}

199
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java Normal file
View File

@@ -0,0 +1,199 @@
package dev.cyberarm.minibots.red_crab;
import com.arcrobotics.ftclib.hardware.motors.Motor;
import com.arcrobotics.ftclib.hardware.motors.MotorEx;
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.CurrentUnit;
import dev.cyberarm.engine.V2.CyberarmEngine;
public class RedCrabMinibot {
/// CLAW ARM ///
public static final int ClawArm_STOW = 0;
public static final int ClawArm_DEPOSIT = 1;
public static final int ClawArm_COLLECT = 2;
/// TUNING CONSTANTS ///
public static final double DRIVETRAIN_MAX_SPEED = 0.5;
public static final double WINCH_MAX_SPEED = 0.5;
public static final double CLAW_ARM_STOW_ANGLE = 0.0;
public static final double CLAW_ARM_DEPOSIT_ANGLE = 120.0;
public static final double CLAW_ARM_COLLECT_ANGLE = 150.0;
public static final double CLAW_WRIST_STOW_POSITION = 0.5;
public static final double CLAW_WRIST_DEPOSIT_POSITION = 0.5;
public static final double CLAW_WRIST_COLLECT_POSITION = 0.5;
public static final double CLAW_LEFT_CLOSED_POSITION = 0.5;
public static final double CLAW_LEFT_OPEN_POSITION = 0.5;
public static final double CLAW_RIGHT_CLOSED_POSITION = 0.5;
public static final double CLAW_RIGHT_OPEN_POSITION = 0.5;
public static final double DRONE_LATCH_INITIAL_POSITION = 0.5;
public static final double DRONE_LATCH_LAUNCH_POSITION = 0.5;
public static final int DRONE_LAUNCH_CONFIRMATION_TIME_MS = 1_000;
public static final double HOOK_ARM_STOW_POSITION = 0.0;
public static final double HOOK_ARM_UP_POSITION = 0.5;
/// MOTOR CONSTANTS ///
public static final int CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION = 4;
public static final double CLAW_ARM_MOTOR_GEAR_RATIO = 72;
/// HARDWARE ///
public final IMU imu;
public final MotorEx frontLeft, frontRight, backLeft, backRight, winch, clawArm;
public final Servo leftClaw, rightClaw, clawWrist, droneLatch, hookArm;
final CyberarmEngine engine;
public RedCrabMinibot() {
engine = CyberarmEngine.instance;
/// IMU ///
/// ------------------------------------------------------------------------------------ ///
imu = engine.hardwareMap.get(IMU.class, "imu"); // | Ctrl Hub, I2C Port: 0
IMU.Parameters parameters = new IMU.Parameters(
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.LEFT,
RevHubOrientationOnRobot.UsbFacingDirection.UP));
imu.initialize(parameters);
/// DRIVE TRAIN ///
/// ------------------------------------------------------------------------------------ ///
frontLeft = new MotorEx(engine.hardwareMap, "frontLeft"); // | Ctrl|Ex Hub, Port: ??
frontRight = new MotorEx(engine.hardwareMap, "frontRight"); // | Ctrl|Ex Hub, Port: ??
backLeft = new MotorEx(engine.hardwareMap, "backLeft"); // | Ctrl|Ex Hub, Port: ??
backRight = new MotorEx(engine.hardwareMap, "backRight"); // | Ctrl|Ex Hub, Port: ??
/// --- (SOFT) RESET MOTOR ENCODERS
frontLeft.resetEncoder();
frontRight.resetEncoder();
backLeft.resetEncoder();
backRight.resetEncoder();
/// --- MOTOR DIRECTIONS
frontLeft.motorEx.setDirection(DcMotorSimple.Direction.FORWARD);
frontRight.motorEx.setDirection(DcMotorSimple.Direction.REVERSE);
backLeft.motorEx.setDirection(DcMotorSimple.Direction.FORWARD);
backRight.motorEx.setDirection(DcMotorSimple.Direction.REVERSE);
/// --- MOTOR BRAKING MODE
/// --- NOTE: Having BRAKE mode set for drivetrain helps with consistently of control
frontLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
frontRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
backLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
backRight.setZeroPowerBehavior((Motor.ZeroPowerBehavior.BRAKE));
/// WINCH ///
/// ------------------------------------------------------------------------------------ ///
winch = new MotorEx(engine.hardwareMap, "winch"); // | Ctrl|Ex Hub, Port: ??
/// --- (SOFT) MOTOR ENCODER RESET
winch.resetEncoder();
/// --- MOTOR DIRECTION
/// --- NOTE: Unknown if FORWARD or REVERSE is correct
winch.motorEx.setDirection(DcMotorSimple.Direction.FORWARD);
/// CLAW and Co. ///
/// ------------------------------------------------------------------------------------ ///
clawArm = new MotorEx(engine.hardwareMap, "clawArm"); // | Ctrl|Ex Hub, Port: ??
clawWrist = engine.hardwareMap.servo.get("clawWrist"); // | Ctrl|Ex Hub, Port: ??
leftClaw = engine.hardwareMap.servo.get("leftClaw"); // | Ctrl|Ex Hub, Port: ??
rightClaw = engine.hardwareMap.servo.get("rightClaw"); // | Ctrl|Ex Hub, Port: ??
/// --- Claw Arm Motor
/// --- --- (SOFT) RESET MOTOR ENCODER
clawArm.resetEncoder();
/// --- --- DIRECTION
clawArm.motorEx.setDirection(DcMotorSimple.Direction.FORWARD);
/// --- --- BRAKING
/// --- --- NOTE: This won't hold back much, if anything, but its a small help, maybe? 😃
clawArm.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
/// --- Claws
/// --- --- Wrist
clawWrist.setDirection(Servo.Direction.FORWARD);
// clawWrist.setPosition(CLAW_WRIST_INITIAL_POSITION);
/// --- --- Left
leftClaw.setDirection(Servo.Direction.FORWARD);
// leftClaw.setPosition((CLAW_LEFT_CLOSED_POSITION));
/// --- --- Right
leftClaw.setDirection(Servo.Direction.FORWARD);
// leftClaw.setPosition((CLAW_RIGHT_CLOSED_POSITION));
/// DRONE LATCH ///
droneLatch = engine.hardwareMap.servo.get("droneLatch");
droneLatch.setDirection(Servo.Direction.FORWARD);
// droneLatch.setPosition(DRONE_LATCH_INITIAL_POSITION);
/// HOOK ARM ///
hookArm = engine.hardwareMap.servo.get("hookArm");
hookArm.setDirection(Servo.Direction.FORWARD);
// hookArm.setPosition(HOOK_ARM_STOW_POSITION);
}
public void standardTelemetry() {
engine.telemetry.addLine();
engine.telemetry.addLine("Motors");
engine.telemetry.addData(
"Front Left",
"Power: %.2f, Current: %.2f mAmp, Position: %d",
frontLeft.motorEx.getPower(),
frontLeft.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
frontLeft.motorEx.getCurrentPosition());
engine.telemetry.addData(
"Front Right",
"Power: %.2f, Current: %.2f mAmp, Position: %d",
frontRight.motorEx.getPower(),
frontRight.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
frontRight.motorEx.getCurrentPosition());
engine.telemetry.addData(
"Back Left",
"Power: %.2f, Current: %.2f mAmp, Position: %d",
backLeft.motorEx.getPower(),
backLeft.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
backLeft.motorEx.getCurrentPosition());
engine.telemetry.addData(
"Back Right",
"Power: %.2f, Current: %.2f mAmp, Position: %d",
backRight.motorEx.getPower(),
backRight.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
backRight.motorEx.getCurrentPosition());
engine.telemetry.addLine();
engine.telemetry.addData(
"Winch",
"Power: %.2f, Current: %.2f mAmp, Position: %d",
winch.motorEx.getPower(),
winch.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
winch.motorEx.getCurrentPosition());
engine.telemetry.addLine();
engine.telemetry.addData(
"Claw Arm",
"Power: %.2f, Current: %.2f mAmp, Position: %d",
clawArm.motorEx.getPower(),
clawArm.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
clawArm.motorEx.getCurrentPosition());
engine.telemetry.addLine();
engine.telemetry.addLine("Servos");
engine.telemetry.addData("Claw Wrist", clawWrist.getPosition());
engine.telemetry.addData("Left Claw", leftClaw.getPosition());
engine.telemetry.addData("Right Claw", rightClaw.getPosition());
engine.telemetry.addLine();
engine.telemetry.addData("Drone Latch", droneLatch.getPosition());
engine.telemetry.addLine();
engine.telemetry.addData("Hook Arm", hookArm.getPosition());
engine.telemetry.addLine();
}
}

15
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java Normal file
View File

@@ -0,0 +1,15 @@
package dev.cyberarm.minibots.red_crab.engines;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
import dev.cyberarm.minibots.red_crab.states.Pilot;
@TeleOp(name = "Cyberarm Red Crab TeleOp", group = "MINIBOT")
public class RedCrabTeleOpEngine extends CyberarmEngine {
@Override
public void setup() {
addState(new Pilot(new RedCrabMinibot()));
}
}

180
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java Normal file
View File

@@ -0,0 +1,180 @@
package dev.cyberarm.minibots.red_crab.states;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import dev.cyberarm.engine.V2.CyberarmState;
import dev.cyberarm.engine.V2.Utilities;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
public class Pilot extends CyberarmState {
private final RedCrabMinibot robot;
private boolean leftClawOpen = false;
private boolean rightClawOpen = false;
private int clawArmPosition = RedCrabMinibot.ClawArm_STOW;
private boolean hookArmUp = false;
private boolean droneLaunchAuthorized = false;
private boolean droneLaunchRequested = false;
private double droneLastLaunchRequestStartMS = 0;
public Pilot(RedCrabMinibot robot) {
this.robot = robot;
}
@Override
public void exec() {
drivetrain();
// clawArmAndWristController();
// clawController();
// droneLatchController();
// hookArmController();
// winchController();
}
@Override
public void buttonDown(Gamepad gamepad, String button) {
if (gamepad == engine.gamepad1) {
switch (button) {
case "y":
droneLaunchRequested = true;
droneLastLaunchRequestStartMS = runTime();
break;
case "x":
hookArmUp = true;
break;
case "b":
hookArmUp = false;
break;
case "guide":
robot.imu.resetYaw();
break;
case "left_bumper":
leftClawOpen = !leftClawOpen;
break;
case "right_bumper":
rightClawOpen = !rightClawOpen;
break;
case "dpad_up":
clawArmPosition = RedCrabMinibot.ClawArm_COLLECT;
break;
case "dpad_down":
clawArmPosition = RedCrabMinibot.ClawArm_DEPOSIT;
break;
case "dpad_left":
case "dpad_right":
clawArmPosition = RedCrabMinibot.ClawArm_STOW;
break;
}
}
}
@Override
public void buttonUp(Gamepad gamepad, String button) {
if (gamepad == engine.gamepad1) {
switch (button) {
case "y":
droneLaunchRequested = false;
droneLastLaunchRequestStartMS = runTime();
break;
}
}
}
@Override
public void telemetry() {
robot.standardTelemetry();
}
private void drivetrain() {
// 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;
double maxPower = RedCrabMinibot.DRIVETRAIN_MAX_SPEED;
robot.frontLeft.motorEx.setPower(frontLeftPower * maxPower);
robot.backLeft.motorEx.setPower(backLeftPower * maxPower);
robot.frontRight.motorEx.setPower(frontRightPower * maxPower);
robot.backRight.motorEx.setPower(backRightPower * maxPower);
}
private void clawArmAndWristController() {
switch (clawArmPosition) {
case RedCrabMinibot.ClawArm_STOW:
robot.clawArm.setTargetPosition(Utilities.motorAngle(
RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO,
RedCrabMinibot.CLAW_ARM_STOW_ANGLE));
robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_STOW_POSITION);
break;
case RedCrabMinibot.ClawArm_DEPOSIT:
robot.clawArm.setTargetPosition(Utilities.motorAngle(
RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO,
RedCrabMinibot.CLAW_ARM_DEPOSIT_ANGLE));
robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_DEPOSIT_POSITION);
break;
case RedCrabMinibot.ClawArm_COLLECT:
robot.clawArm.setTargetPosition(Utilities.motorAngle(
RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO,
RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE));
robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_COLLECT_POSITION);
break;
}
}
private void clawController() {
robot.leftClaw.setPosition((leftClawOpen ? RedCrabMinibot.CLAW_LEFT_OPEN_POSITION : RedCrabMinibot.CLAW_LEFT_CLOSED_POSITION));
robot.rightClaw.setPosition((rightClawOpen ? RedCrabMinibot.CLAW_RIGHT_OPEN_POSITION : RedCrabMinibot.CLAW_RIGHT_CLOSED_POSITION));
}
private void droneLatchController() {
if (droneLaunchRequested && runTime() - droneLastLaunchRequestStartMS >= RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS)
droneLaunchAuthorized = true;
if (droneLaunchAuthorized) {
robot.droneLatch.setPosition(RedCrabMinibot.DRONE_LATCH_LAUNCH_POSITION);
}
}
private void hookArmController() {
robot.hookArm.setPosition((hookArmUp ? RedCrabMinibot.HOOK_ARM_UP_POSITION : RedCrabMinibot.HOOK_ARM_STOW_POSITION));
}
private void winchController() {
if (engine.gamepad1.right_trigger > 0) {
robot.winch.motorEx.setPower(engine.gamepad1.right_trigger * RedCrabMinibot.WINCH_MAX_SPEED);
} else if (engine.gamepad1.left_trigger > 0) {
robot.winch.motorEx.setPower(engine.gamepad1.left_trigger * RedCrabMinibot.WINCH_MAX_SPEED);
} else {
robot.winch.motorEx.setPower(0);
}
}
}