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

RedCrab: 'Finished' adapting code to use velocity instead of raw power, verified distance math 😄, claw arm still needs work, misc.

Browse Source
This commit is contained in:
Cyberarm
2023-12-20 20:00:10 -06:00
parent a9dfea93ed
commit e803df0cd9
5 changed files with 237 additions and 142 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -10,6 +10,7 @@ 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.PIDFCoefficients;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.Range;
@@ -55,7 +56,7 @@ public class RedCrabMinibot {
public static double CLAW_ARM_COLLECT_FLOAT_ANGLE = 180.0;
public static double CLAW_ARM_COLLECT_ANGLE = 200.0;
public static final double WINCH_MAX_SPEED = 0.5;
public static double WINCH_MAX_SPEED = 0.5;
public static double CLAW_WRIST_STOW_POSITION = 0.7;
public static double CLAW_WRIST_DEPOSIT_POSITION = 0.64;
@@ -80,12 +81,10 @@ public class RedCrabMinibot {
/// HARDWARE ///
public final IMU imu;
public final MotorEx frontLeft, frontRight, backLeft, backRight, winch;
public final DcMotorEx frontLeft, frontRight, backLeft, backRight, winch;
public final DcMotorEx clawArm;
public final Servo leftClaw, rightClaw, clawWrist, droneLatch, hookArm;
public final MotorGroup left, right;
final CyberarmEngine engine;
public TimeCraftersConfiguration config;
@@ -119,7 +118,7 @@ public class RedCrabMinibot {
/// IMU ///
/// ------------------------------------------------------------------------------------ ///
imu = engine.hardwareMap.get(IMU.class, "imu"); // | Ctrl Hub, I2C Port: 0
imu = engine.hardwareMap.get(IMU.class, "imu"); // | Control Hub, I2C Port: 0
IMU.Parameters parameters = new IMU.Parameters(
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.RIGHT,
@@ -132,33 +131,29 @@ public class RedCrabMinibot {
/// 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: ??
frontLeft = (DcMotorEx) engine.hardwareMap.dcMotor.get("frontLeft"); // | Expansion Hub, Motor Port: 2
frontRight = (DcMotorEx) engine.hardwareMap.dcMotor.get("frontRight"); // | Expansion Hub, Motor Port: 3
backLeft = (DcMotorEx) engine.hardwareMap.dcMotor.get("backLeft"); // | Expansion Hub, Motor Port: 3
backRight = (DcMotorEx) engine.hardwareMap.dcMotor.get("backRight"); // | Expansion Hub, Motor Port: 2
/// --- (SOFT) RESET MOTOR ENCODERS
frontLeft.resetEncoder();
frontRight.resetEncoder();
backLeft.resetEncoder();
backRight.resetEncoder();
/// --- RESET MOTOR ENCODERS
frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
frontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
backLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
backRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
/// --- MOTOR DIRECTIONS
frontLeft.motorEx.setDirection(DcMotorSimple.Direction.REVERSE);
frontRight.motorEx.setDirection(DcMotorSimple.Direction.FORWARD);
backLeft.motorEx.setDirection(DcMotorSimple.Direction.REVERSE);
backRight.motorEx.setDirection(DcMotorSimple.Direction.FORWARD);
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
frontRight.setDirection(DcMotorSimple.Direction.FORWARD);
backLeft.setDirection(DcMotorSimple.Direction.REVERSE);
backRight.setDirection(DcMotorSimple.Direction.FORWARD);
/// --- 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));
/// --- MOTOR GROUPS
left = new MotorGroup(frontLeft, backLeft);
right = new MotorGroup(frontRight, backRight);
frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
/// --- MOTOR DISTANCE PER TICK
double distancePerTick = Utilities.ticksToUnit(
@@ -168,35 +163,33 @@ public class RedCrabMinibot {
DistanceUnit.MM,
1);
frontLeft.setDistancePerPulse(distancePerTick);
frontRight.setDistancePerPulse(distancePerTick);
backLeft.setDistancePerPulse(distancePerTick);
backRight.setDistancePerPulse(distancePerTick);
/// --- RUN MODE
frontLeft.setRunMode(Motor.RunMode.VelocityControl);
frontRight.setRunMode(Motor.RunMode.VelocityControl);
backLeft.setRunMode(Motor.RunMode.VelocityControl);
backRight.setRunMode(Motor.RunMode.VelocityControl);
frontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
frontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
/// WINCH ///
/// ------------------------------------------------------------------------------------ ///
winch = new MotorEx(engine.hardwareMap, "winch"); // | Ctrl|Ex Hub, Port: ??
winch = (DcMotorEx) engine.hardwareMap.dcMotor.get("winch"); // | Expansion Hub, Motor Port: 0
/// --- (SOFT) MOTOR ENCODER RESET
winch.resetEncoder();
/// --- MOTOR ENCODER RESET
winch.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
/// --- MOTOR DIRECTION
/// --- NOTE: Unknown if FORWARD or REVERSE is correct
winch.motorEx.setDirection(DcMotorSimple.Direction.FORWARD);
winch.setDirection(DcMotorSimple.Direction.FORWARD);
/// --- RUN MODE
winch.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
/// CLAW and Co. ///
/// ------------------------------------------------------------------------------------ ///
clawArmPIDFController = new PIDFController(0.4, 0.01, 0.1, 0.0);
clawArm = (DcMotorEx) engine.hardwareMap.dcMotor.get("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: ??
clawArm = (DcMotorEx) engine.hardwareMap.dcMotor.get("clawArm"); // | Expansion Hub, Motor Port: 1
clawWrist = engine.hardwareMap.servo.get("clawWrist"); // | Control Hub, Servo Port: 0
leftClaw = engine.hardwareMap.servo.get("leftClaw"); // | Control Hub, Servo Port: 2
rightClaw = engine.hardwareMap.servo.get("rightClaw"); // | Control Hub, Servo Port: 1
/// --- Claw Arm Motor
/// --- --- (SOFT) RESET MOTOR ENCODER
@@ -207,9 +200,9 @@ public class RedCrabMinibot {
/// --- --- NOTE: This won't hold back much, if anything, but its a small help, maybe? 😃
clawArm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
/// --- --- Run Mode
clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
clawArm.setTargetPositionTolerance(CLAW_ARM_POSITION_TOLERANCE);
clawArm.setTargetPosition(0);
clawArm.setTargetPositionTolerance(CLAW_ARM_POSITION_TOLERANCE);
clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
/// --- Claws
/// --- --- Wrist
@@ -223,12 +216,12 @@ public class RedCrabMinibot {
rightClaw.setPosition((CLAW_RIGHT_CLOSED_POSITION));
/// DRONE LATCH ///
droneLatch = engine.hardwareMap.servo.get("droneLatch"); // | Ctrl|Ex Hub, Port: ??
droneLatch = engine.hardwareMap.servo.get("droneLatch"); // | Expansion Hub, Servo Port: 0
droneLatch.setDirection(Servo.Direction.FORWARD);
droneLatch.setPosition(DRONE_LATCH_INITIAL_POSITION);
/// HOOK ARM ///
hookArm = engine.hardwareMap.servo.get("hookArm"); // | Ctrl|Ex Hub, Port: ??
hookArm = engine.hardwareMap.servo.get("hookArm"); // | Control Hub, Servo Port: 3
hookArm.setDirection(Servo.Direction.FORWARD);
// hookArm.setPosition(HOOK_ARM_STOW_POSITION); // LEAVE OFF:
@@ -263,6 +256,10 @@ public class RedCrabMinibot {
RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO = config.variable("Robot", "ClawArm_Tuning", "gear_ratio").value();
RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION = config.variable("Robot", "ClawArm_Tuning", "motor_ticks").value();
/// WINCH
RedCrabMinibot.WINCH_MAX_SPEED = config.variable("Robot", "Winch_Tuning", "max_speed").value();
/// CLAW WRIST
RedCrabMinibot.CLAW_WRIST_STOW_POSITION = config.variable("Robot", "ClawWrist_Tuning", "stow_position").value();
RedCrabMinibot.CLAW_WRIST_DEPOSIT_POSITION = config.variable("Robot", "ClawWrist_Tuning", "deposit_position").value();
@@ -280,8 +277,8 @@ public class RedCrabMinibot {
RedCrabMinibot.HOOK_ARM_UP_POSITION = config.variable("Robot", "HookArm_Tuning", "up_position").value();
/// DRONE LATCH
RedCrabMinibot.DRONE_LATCH_LAUNCH_POSITION = config.variable("Robot", "DroneLauncher_Tuning", "initial_position").value();
RedCrabMinibot.DRONE_LATCH_INITIAL_POSITION = config.variable("Robot", "DroneLauncher_Tuning", "launch_position").value();
RedCrabMinibot.DRONE_LATCH_INITIAL_POSITION = config.variable("Robot", "DroneLauncher_Tuning", "initial_position").value();
RedCrabMinibot.DRONE_LATCH_LAUNCH_POSITION = config.variable("Robot", "DroneLauncher_Tuning", "launch_position").value();
RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS = config.variable("Robot", "DroneLauncher_Tuning", "launch_confirmation_time_ms").value();
}
@@ -291,56 +288,96 @@ public class RedCrabMinibot {
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());
"Power: %.2f, Current: %.2f mAmp, Position: %d, Velocity: %.2f (%.2f mm/s)",
frontLeft.getPower(),
frontLeft.getCurrent(CurrentUnit.MILLIAMPS),
frontLeft.getCurrentPosition(),
frontLeft.getVelocity(),
Utilities.ticksToUnit(
DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION,
DRIVETRAIN_GEAR_RATIO,
DRIVETRAIN_WHEEL_DIAMETER_MM,
DistanceUnit.MM,
(int)frontLeft.getVelocity()));
engine.telemetry.addLine();
engine.telemetry.addData(
"Front Right",
"Power: %.2f, Current: %.2f mAmp, Position: %d",
frontRight.motorEx.getPower(),
frontRight.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
frontRight.motorEx.getCurrentPosition());
"Power: %.2f, Current: %.2f mAmp, Position: %d, Velocity: %.2f (%.2f mm/s)",
frontRight.getPower(),
frontRight.getCurrent(CurrentUnit.MILLIAMPS),
frontRight.getCurrentPosition(),
frontRight.getVelocity(),
Utilities.ticksToUnit(
DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION,
DRIVETRAIN_GEAR_RATIO,
DRIVETRAIN_WHEEL_DIAMETER_MM,
DistanceUnit.MM,
(int)frontLeft.getVelocity()));
engine.telemetry.addLine();
engine.telemetry.addData(
"Back Left",
"Power: %.2f, Current: %.2f mAmp, Position: %d",
backLeft.motorEx.getPower(),
backLeft.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
backLeft.motorEx.getCurrentPosition());
"Power: %.2f, Current: %.2f mAmp, Position: %d, Velocity: %.2f (%.2f mm/s)",
backLeft.getPower(),
backLeft.getCurrent(CurrentUnit.MILLIAMPS),
backLeft.getCurrentPosition(),
backLeft.getVelocity(),
Utilities.ticksToUnit(
DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION,
DRIVETRAIN_GEAR_RATIO,
DRIVETRAIN_WHEEL_DIAMETER_MM,
DistanceUnit.MM,
(int)backLeft.getVelocity()));
engine.telemetry.addLine();
engine.telemetry.addData(
"Back Right",
"Power: %.2f, Current: %.2f mAmp, Position: %d",
backRight.motorEx.getPower(),
backRight.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
backRight.motorEx.getCurrentPosition());
"Power: %.2f, Current: %.2f mAmp, Position: %d, Velocity: %.2f (%.2f mm/s)",
backRight.getPower(),
backRight.getCurrent(CurrentUnit.MILLIAMPS),
backRight.getCurrentPosition(),
backRight.getVelocity(),
Utilities.ticksToUnit(
DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION,
DRIVETRAIN_GEAR_RATIO,
DRIVETRAIN_WHEEL_DIAMETER_MM,
DistanceUnit.MM,
(int)backRight.getVelocity()));
engine.telemetry.addLine();
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());
"Power: %.2f, Current: %.2f mAmp, Position: %d, Velocity: %.2f (%.2f mm/s)",
winch.getPower(),
winch.getCurrent(CurrentUnit.MILLIAMPS),
winch.getCurrentPosition(),
winch.getVelocity(),
Utilities.ticksToUnit(
DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION,
DRIVETRAIN_GEAR_RATIO,
DRIVETRAIN_WHEEL_DIAMETER_MM,
DistanceUnit.MM,
(int)winch.getVelocity()));
engine.telemetry.addLine();
PIDFCoefficients clawArmPIDFPosition = clawArm.getPIDFCoefficients(DcMotor.RunMode.RUN_TO_POSITION);
PIDFCoefficients clawArmPIDFEncoder = clawArm.getPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER);
engine.telemetry.addData(
"Claw Arm",
"Power: %.2f, Current: %.2f mAmp, Position: %d, Velocity: %.2f",
"Power: %.2f, Current: %.2f mAmp, Position: %d, Angle: %.2f, Velocity: %.2f (%.2f degrees/s)",
clawArm.getPower(),
clawArm.getCurrent(CurrentUnit.MILLIAMPS),
clawArm.getCurrentPosition(),
clawArm.getVelocity());
Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, clawArm.getCurrentPosition()),
clawArm.getVelocity(),
Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, (int)clawArm.getVelocity()));
engine.telemetry.addData(
" PIDF", "P: %.4f, I: %.4f, D: %.4f, F: %.4f",
clawArmPIDFController.getP(),
clawArmPIDFController.getI(),
clawArmPIDFController.getD(),
clawArmPIDFController.getF());
engine.telemetry.addData(
" PIDF+", "PosError: %.4f, velError: %.4f, Period: %.4f",
clawArmPIDFController.getPositionError(),
clawArmPIDFController.getVelocityError(),
clawArmPIDFController.getPeriod());
" PIDF", "P: %.4f, I: %.4f, D: %.4f, F: %.4f, PosP: %.4f",
clawArmPIDFEncoder.p,
clawArmPIDFEncoder.i,
clawArmPIDFEncoder.d,
clawArmPIDFEncoder.f,
clawArmPIDFPosition.p);
engine.telemetry.addLine();
engine.telemetry.addLine("Servos");
@@ -399,4 +436,20 @@ public class RedCrabMinibot {
clawArm.setVelocity(velocity);
}
public double distanceMM(DcMotorEx motor) {
return Utilities.ticksToUnit(
RedCrabMinibot.DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.DRIVETRAIN_GEAR_RATIO,
RedCrabMinibot.DRIVETRAIN_WHEEL_DIAMETER_MM,
DistanceUnit.MM,
motor.getCurrentPosition()
);
}
public boolean atTargetPosition(DcMotorEx motor, double travelledDistanceMM, double toleranceMM) {
double distanceMM = distanceMM(motor);
return Utilities.isBetween(distanceMM, travelledDistanceMM - toleranceMM, travelledDistanceMM + toleranceMM);
}
}

2
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/cyberarm_RedCrab.json
View File

File diff suppressed because one or more lines are too long

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

@@ -1,7 +1,11 @@
package dev.cyberarm.minibots.red_crab.states;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.util.Range;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import dev.cyberarm.engine.V2.CyberarmState;
import dev.cyberarm.engine.V2.Utilities;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
@@ -50,30 +54,45 @@ public class Move extends CyberarmState {
throw(new RuntimeException("INVALID Initial IMU value!"));
}
int toleranceTicks = Utilities.unitToTicks(
RedCrabMinibot.DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.DRIVETRAIN_GEAR_RATIO,
RedCrabMinibot.DRIVETRAIN_WHEEL_DIAMETER_MM,
DistanceUnit.MM,
toleranceMM);
int distanceTicks = Utilities.unitToTicks(
RedCrabMinibot.DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.DRIVETRAIN_GEAR_RATIO,
RedCrabMinibot.DRIVETRAIN_WHEEL_DIAMETER_MM,
DistanceUnit.MM,
toleranceMM);
robot.frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.frontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.backLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.backRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.frontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.frontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.backLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.backRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.frontLeft.setTargetPositionTolerance(toleranceTicks);
robot.frontRight.setTargetPositionTolerance(toleranceTicks);
robot.backLeft.setTargetPositionTolerance(toleranceTicks);
robot.backRight.setTargetPositionTolerance(toleranceTicks);
if (strafe) {
robot.frontLeft.resetEncoder();
robot.frontRight.resetEncoder();
robot.backLeft.resetEncoder();
robot.backRight.resetEncoder();
robot.frontLeft.setTargetPosition(distanceTicks);
robot.frontRight.setTargetPosition(-distanceTicks);
robot.backLeft.setTargetPosition(-distanceTicks);
robot.backRight.setTargetPosition(distanceTicks);
robot.frontLeft.setPositionTolerance(toleranceMM);
robot.frontRight.setPositionTolerance(toleranceMM);
robot.backLeft.setPositionTolerance(toleranceMM);
robot.backRight.setPositionTolerance(toleranceMM);
robot.frontLeft.setTargetDistance(distanceMM);
robot.frontRight.setTargetDistance(-distanceMM);
robot.backLeft.setTargetDistance(-distanceMM);
robot.backRight.setTargetDistance(distanceMM);
} else {
robot.left.resetEncoder();
robot.right.resetEncoder();
robot.left.setPositionTolerance(toleranceMM);
robot.right.setPositionTolerance(toleranceMM);
robot.left.setTargetDistance(distanceMM);
robot.right.setTargetDistance(distanceMM);
robot.frontLeft.setTargetPosition(distanceTicks);
robot.frontRight.setTargetPosition(distanceTicks);
robot.backLeft.setTargetPosition(distanceTicks);
robot.backRight.setTargetPosition(distanceTicks);
}
}
@@ -93,13 +112,13 @@ public class Move extends CyberarmState {
engine.telemetry.addData("lerp MM UP", lerpMM_UP);
engine.telemetry.addData("lerp MM DOWN", lerpMM_DOWN);
engine.telemetry.addData("Distance MM", distanceMM);
engine.telemetry.addData("Distance Travelled MM", robot.frontLeft.getDistance());
engine.telemetry.addData("Distance Travelled MM", robot.distanceMM(robot.frontLeft));
engine.telemetry.addData("Timeout MS", timeoutMS);
progressBar(20, runTime() / timeoutMS);
}
private void tankMove(){
double travelledDistance = Math.abs(robot.frontLeft.getDistance());
double travelledDistance = Math.abs(robot.distanceMM(robot.frontLeft));
double velocity = lerpVelocity(travelledDistance);
double angleDiff = Utilities.angleDiff(initialHeadingDegrees, Utilities.facing(robot.imu));
@@ -114,21 +133,25 @@ public class Move extends CyberarmState {
rightVelocity += correctiveVelocity;
}
robot.left.set(leftVelocity);
robot.right.set(rightVelocity);
robot.frontLeft.setVelocity(leftVelocity);
robot.frontRight.setVelocity(rightVelocity);
robot.backLeft.setVelocity(leftVelocity);
robot.backRight.setVelocity(leftVelocity);
if (runTime() >= timeoutMS ||
(robot.frontLeft.atTargetPosition() || robot.frontRight.atTargetPosition()) ||
Math.abs(robot.frontLeft.getDistance()) >= Math.abs(distanceMM)) {
robot.left.set(0);
robot.right.set(0);
(atTargetPosition(robot.frontLeft) || atTargetPosition(robot.frontRight)) ||
Math.abs(robot.distanceMM(robot.frontLeft)) >= Math.abs(distanceMM)) {
robot.frontLeft.setVelocity(0);
robot.frontRight.setVelocity(0);
robot.backLeft.setVelocity(0);
robot.backRight.setVelocity(0);
this.finished();
}
}
private void strafeMove() {
double travelledDistance = Math.abs(robot.frontLeft.getDistance());
double travelledDistance = Math.abs(robot.distanceMM(robot.frontLeft));
double velocity = lerpVelocity(travelledDistance);
double angleDiff = Utilities.angleDiff(initialHeadingDegrees, Utilities.facing(robot.imu));
@@ -143,17 +166,17 @@ public class Move extends CyberarmState {
backVelocity += correctiveVelocity;
}
robot.frontLeft.set(frontVelocity);
robot.frontRight.set(-frontVelocity);
robot.backLeft.set(-backVelocity);
robot.backRight.set(backVelocity);
robot.frontLeft.setVelocity(frontVelocity);
robot.frontRight.setVelocity(-frontVelocity);
robot.backLeft.setVelocity(-backVelocity);
robot.backRight.setVelocity(backVelocity);
if (runTime() >= timeoutMS || (robot.frontLeft.atTargetPosition() || robot.backRight.atTargetPosition()) ||
Math.abs(robot.frontLeft.getDistance()) >= Math.abs(distanceMM) || Math.abs(robot.backRight.getDistance()) >= Math.abs(distanceMM)) {
robot.frontLeft.set(0);
robot.frontRight.set(0);
robot.backLeft.set(0);
robot.backRight.set(0);
if (runTime() >= timeoutMS || (atTargetPosition(robot.frontLeft) || atTargetPosition(robot.backRight)) ||
Math.abs(robot.distanceMM(robot.frontLeft)) >= Math.abs(distanceMM) || Math.abs(robot.distanceMM(robot.backRight)) >= Math.abs(distanceMM)) {
robot.frontLeft.setVelocity(0);
robot.frontRight.setVelocity(0);
robot.backLeft.setVelocity(0);
robot.backRight.setVelocity(0);
this.finished();
}
@@ -175,4 +198,10 @@ public class Move extends CyberarmState {
return lerpVelocity;
}
private boolean atTargetPosition(DcMotorEx motor) {
double travelledDistanceMM = robot.distanceMM(motor);
return Utilities.isBetween(travelledDistanceMM, distanceMM - toleranceMM, distanceMM + toleranceMM);
}
}

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

@@ -133,10 +133,10 @@ public class Pilot extends CyberarmState {
RedCrabMinibot.DRIVETRAIN_VELOCITY_SLOW_MM);
double velocity = robotSlowMode ? slowVelocity : maxVelocity;
robot.frontLeft.set(frontLeftPower * velocity);
robot.backLeft.set(backLeftPower * velocity);
robot.frontRight.set(frontRightPower * velocity);
robot.backRight.set(backRightPower * velocity);
robot.frontLeft.setVelocity(frontLeftPower * velocity);
robot.backLeft.setVelocity(backLeftPower * velocity);
robot.frontRight.setVelocity(frontRightPower * velocity);
robot.backRight.setVelocity(backRightPower * velocity);
}
private void clawArmAndWristController() {
@@ -207,11 +207,11 @@ public class Pilot extends CyberarmState {
private void winchController() {
if (engine.gamepad1.right_trigger > 0) {
robot.winch.motorEx.setPower(engine.gamepad1.right_trigger * RedCrabMinibot.WINCH_MAX_SPEED);
robot.winch.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);
robot.winch.setPower(-engine.gamepad1.left_trigger * RedCrabMinibot.WINCH_MAX_SPEED);
} else {
robot.winch.motorEx.setPower(0);
robot.winch.setPower(0);
}
}
}

39
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Rotate.java
View File

@@ -2,16 +2,17 @@ package dev.cyberarm.minibots.red_crab.states;
import com.qualcomm.robotcore.util.Range;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import dev.cyberarm.engine.V2.CyberarmState;
import dev.cyberarm.engine.V2.Utilities;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
import dev.cyberarm.minibots.yellow.YellowMinibot;
public class Rotate extends CyberarmState {
final private RedCrabMinibot robot;
final private String groupName, actionName;
final private double maxPower, minPower, lerpDegrees, headingDegrees, toleranceDegrees;
final private double maxVelocityMM, minVelocityMM, lerpDegrees, headingDegrees, toleranceDegrees;
final private int timeoutMS;
private boolean commitToRotation = false;
@@ -21,8 +22,8 @@ public class Rotate extends CyberarmState {
this.groupName = groupName;
this.actionName = actionName;
this.minPower = robot.config.variable(groupName, actionName, "minPower").value();
this.maxPower = robot.config.variable(groupName, actionName, "maxPower").value();
this.maxVelocityMM = robot.config.variable(groupName, actionName, "maxVelocityMM").value();
this.minVelocityMM = robot.config.variable(groupName, actionName, " minVelocityMM").value();
this.lerpDegrees = robot.config.variable(groupName, actionName, "lerpDEGREES").value();
this.headingDegrees = robot.config.variable(groupName, actionName, "headingDEGREES").value();
@@ -33,26 +34,38 @@ public class Rotate extends CyberarmState {
@Override
public void exec() {
double angleDiff = Utilities.angleDiff(Utilities.facing(robot.imu) + Utilities.turnRate(robot.imu), headingDegrees);
double angleDiff = Utilities.angleDiff(Utilities.facing(robot.imu), headingDegrees);
if (Math.abs(angleDiff) <= toleranceDegrees || runTime() >= timeoutMS) {
robot.left.set(0);
robot.right.set(0);
robot.frontLeft.setPower(0);
robot.frontRight.setPower(0);
robot.backLeft.setPower(0);
robot.backRight.setPower(0);
this.finished();
return;
}
double power = Utilities.lerp(minPower, maxPower, Range.clip(Math.abs(angleDiff) / lerpDegrees, 0.0, 1.0));
double velocityMM = Utilities.lerp(minVelocityMM, maxVelocityMM, Range.clip(Math.abs(angleDiff) / lerpDegrees, 0.0, 1.0));
double velocity = Utilities.unitToTicks(
RedCrabMinibot.DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.DRIVETRAIN_GEAR_RATIO,
RedCrabMinibot.DRIVETRAIN_WHEEL_DIAMETER_MM,
DistanceUnit.MM,
velocityMM);
if (!commitToRotation) {
if (angleDiff < 0) {
robot.left.set(-power);
robot.right.set(power);
robot.frontLeft.setVelocity(-velocity);
robot.frontRight.setVelocity(velocity);
robot.backLeft.setVelocity(-velocity);
robot.backRight.setVelocity(velocity);
} else {
robot.left.set(power);
robot.right.set(-power);
robot.frontLeft.setVelocity(velocity);
robot.frontRight.setVelocity(-velocity);
robot.backLeft.setVelocity(velocity);
robot.backRight.setVelocity(-velocity);
}
}
@@ -64,7 +77,7 @@ public class Rotate extends CyberarmState {
engine.telemetry.addLine();
engine.telemetry.addData("Robot Heading", Utilities.facing(robot.imu));
engine.telemetry.addData("Robot Target Heading", headingDegrees);
engine.telemetry.addData("Robot Angle Diff", Utilities.angleDiff(Utilities.facing(robot.imu) + Utilities.turnRate(robot.imu), headingDegrees));
engine.telemetry.addData("Robot Angle Diff", Utilities.angleDiff(Utilities.facing(robot.imu), headingDegrees));
engine.telemetry.addData("Robot Turn Rate", Utilities.turnRate(robot.imu));
}
}