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

RedCrab: Refactored ClawArm to use a raw PID[F] controller, converted robot constants to get set from config

Browse Source
This commit is contained in:
Cyberarm
2023-12-17 10:22:06 -06:00
parent a4ed6101c4
commit fe99b9f6fd
5 changed files with 143 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,9 +1,12 @@
package dev.cyberarm.minibots.red_crab;
import com.arcrobotics.ftclib.controller.PIDController;
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.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.Servo;
@@ -15,6 +18,8 @@ import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.tfod.TfodProcessor;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
import java.sql.Time;
@@ -30,47 +35,51 @@ public class RedCrabMinibot {
public static final int ClawArm_COLLECT = 3;
/// TUNING CONSTANTS ///
public static final double DRIVETRAIN_MAX_SPEED = 0.5;
public static final double CLAW_ARM_MAX_SPEED = 0.5;
public static final double CLAW_ARM_kP = 0.1;
public static final double CLAW_ARM_POSITION_TOLERANCE = 1.5;
public static double DRIVETRAIN_MAX_SPEED = 0.5;
public static double CLAW_ARM_MAX_SPEED = 0.5;
public static double CLAW_ARM_kP = 0.1;
public static int CLAW_ARM_POSITION_TOLERANCE = 1;
public static double CLAW_ARM_STOW_ANGLE = 45.0; // 45.0
public static double CLAW_ARM_DEPOSIT_ANGLE = 130.0; // 110.0
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 final double CLAW_ARM_STOW_ANGLE = 45.0; // 45.0
public static final double CLAW_ARM_DEPOSIT_ANGLE = 130.0; // 110.0
public static final double CLAW_ARM_COLLECT_FLOAT_ANGLE = 180.0;
public static final double CLAW_ARM_COLLECT_ANGLE = 200.0;
public static final double CLAW_WRIST_STOW_POSITION = 0.7;
public static final double CLAW_WRIST_DEPOSIT_POSITION = 0.64;
public static final double CLAW_WRIST_COLLECT_FLOAT_POSITION = 0.64;
public static final double CLAW_WRIST_COLLECT_POSITION = 0.64;
public static double CLAW_WRIST_STOW_POSITION = 0.7;
public static double CLAW_WRIST_DEPOSIT_POSITION = 0.64;
public static double CLAW_WRIST_COLLECT_FLOAT_POSITION = 0.64;
public static double CLAW_WRIST_COLLECT_POSITION = 0.64;
public static final double CLAW_LEFT_CLOSED_POSITION = 0.2;
public static final double CLAW_LEFT_OPEN_POSITION = 0.5;
public static final double CLAW_RIGHT_CLOSED_POSITION = 0.77;
public static final double CLAW_RIGHT_OPEN_POSITION = 0.5;
public static double CLAW_LEFT_CLOSED_POSITION = 0.2;
public static double CLAW_LEFT_OPEN_POSITION = 0.5;
public static double CLAW_RIGHT_CLOSED_POSITION = 0.77;
public static 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.7;
public static final int DRONE_LAUNCH_CONFIRMATION_TIME_MS = 1_000;
public static double DRONE_LATCH_INITIAL_POSITION = 0.5;
public static double DRONE_LATCH_LAUNCH_POSITION = 0.7;
public static int DRONE_LAUNCH_CONFIRMATION_TIME_MS = 1_000;
public static final double HOOK_ARM_STOW_POSITION = 0.8; // just off of airplane 0.8
public static final double HOOK_ARM_UP_POSITION = 0.4; // streight up4.0
public static double HOOK_ARM_STOW_POSITION = 0.8; // just off of airplane 0.8
public static double HOOK_ARM_UP_POSITION = 0.4; // streight up4.0
/// MOTOR CONSTANTS ///
public static final int CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION = 4;
public static final double CLAW_ARM_MOTOR_GEAR_RATIO = 80; // Technically 72, but there is a lot of slop
public static int CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION = 4;
public static double CLAW_ARM_MOTOR_GEAR_RATIO = 80; // Technically 72, but there is a lot of slop
/// HARDWARE ///
public final IMU imu;
public final MotorEx frontLeft, frontRight, backLeft, backRight, winch, clawArm;
public final MotorEx 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 final TimeCraftersConfiguration config;
public TimeCraftersConfiguration config;
private final PIDController clawArmPIDController;
public enum Path {
LEFT,
@@ -85,6 +94,7 @@ public class RedCrabMinibot {
engine = CyberarmEngine.instance;
config = new TimeCraftersConfiguration("cyberarm_RedCrab");
loadConstants();
if (autonomous) {
tfod = TfodProcessor.easyCreateWithDefaults();
@@ -164,23 +174,23 @@ public class RedCrabMinibot {
/// CLAW and Co. ///
/// ------------------------------------------------------------------------------------ ///
clawArm = new MotorEx(engine.hardwareMap, "clawArm"); // | Ctrl|Ex Hub, Port: ??
clawArmPIDController = new PIDController(0, 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: ??
/// --- Claw Arm Motor
/// --- --- (SOFT) RESET MOTOR ENCODER
clawArm.resetEncoder();
clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
/// --- --- DIRECTION
clawArm.motorEx.setDirection(DcMotorSimple.Direction.FORWARD);
clawArm.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);
clawArm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
/// --- --- Run Mode
clawArm.setRunMode(Motor.RunMode.PositionControl);
clawArm.setPositionCoefficient(CLAW_ARM_kP);
clawArm.setPositionTolerance(CLAW_ARM_POSITION_TOLERANCE);
clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
clawArm.setTargetPositionTolerance(CLAW_ARM_POSITION_TOLERANCE);
clawArm.setTargetPosition(0);
/// --- Claws
@@ -205,6 +215,49 @@ public class RedCrabMinibot {
// hookArm.setPosition(HOOK_ARM_STOW_POSITION); // LEAVE OFF:
}
public void reloadConfig() {
config = new TimeCraftersConfiguration("cyberarm_RedCrab");
loadConstants();
}
private void loadConstants() {
/// Drivetrain
RedCrabMinibot.DRIVETRAIN_MAX_SPEED = config.variable("Robot", "Drivetrain_Tuning", "max_speed").value();
/// CLAW ARM
RedCrabMinibot.CLAW_ARM_MAX_SPEED = config.variable("Robot", "ClawArm_Tuning", "max_speed").value();
RedCrabMinibot.CLAW_ARM_POSITION_TOLERANCE = config.variable("Robot", "ClawArm_Tuning", "tolerance").value();
RedCrabMinibot.CLAW_ARM_STOW_ANGLE = config.variable("Robot", "ClawArm_Tuning", "stow_angle").value();
RedCrabMinibot.CLAW_ARM_DEPOSIT_ANGLE = config.variable("Robot", "ClawArm_Tuning", "collect_float_angle").value();
RedCrabMinibot.CLAW_ARM_COLLECT_FLOAT_ANGLE = config.variable("Robot", "ClawArm_Tuning", "collect_angle").value();
RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE = config.variable("Robot", "ClawArm_Tuning", "collect_angle").value();
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();
/// 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();
RedCrabMinibot.CLAW_WRIST_COLLECT_FLOAT_POSITION = config.variable("Robot", "ClawWrist_Tuning", "float_collect_position").value();
RedCrabMinibot.CLAW_WRIST_COLLECT_POSITION = config.variable("Robot", "ClawWrist_Tuning", "collect_position").value();
/// CLAWS
RedCrabMinibot.CLAW_LEFT_CLOSED_POSITION = config.variable("Robot", "Claw_Tuning", "claw_left_closed_position").value();
RedCrabMinibot.CLAW_LEFT_OPEN_POSITION = config.variable("Robot", "Claw_Tuning", "claw_left_open_position").value();
RedCrabMinibot.CLAW_RIGHT_CLOSED_POSITION = config.variable("Robot", "Claw_Tuning", "claw_right_closed_position").value();
RedCrabMinibot.CLAW_RIGHT_OPEN_POSITION = config.variable("Robot", "Claw_Tuning", "claw_right_open_position").value();
/// HOOK ARM
RedCrabMinibot.HOOK_ARM_STOW_POSITION = config.variable("Robot", "HookArm_Tuning", "stow_position").value();
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_LAUNCH_CONFIRMATION_TIME_MS = config.variable("Robot", "DroneLauncher_Tuning", "launch_confirmation_time_ms").value();
}
public void standardTelemetry() {
engine.telemetry.addLine();
@@ -245,10 +298,11 @@ public class RedCrabMinibot {
engine.telemetry.addData(
"Claw Arm",
"Power: %.2f, Current: %.2f mAmp, Position: %d",
clawArm.motorEx.getPower(),
clawArm.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
clawArm.motorEx.getCurrentPosition());
"Power: %.2f, Current: %.2f mAmp, Position: %d, Velocity: %.2f",
clawArm.getPower(),
clawArm.getCurrent(CurrentUnit.MILLIAMPS),
clawArm.getCurrentPosition(),
clawArm.getVelocity());
engine.telemetry.addLine();
engine.telemetry.addLine("Servos");
@@ -265,4 +319,37 @@ public class RedCrabMinibot {
engine.telemetry.addLine();
}
public void controlClawArm() {
Action action = config.action("Robot", "ClawArm_Tuning");
double p = 0.0, i = 0.0, d = 0.0, f = 0.0;
double ticksInDegree = Utilities.motorAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, 1);
for (Variable v : action.getVariables()) {
switch (v.name.trim()) {
case "kP":
p = v.value();
break;
case "kI":
i = v.value();
break;
case "kD":
d = v.value();
break;
case "kF": // feedback
f = v.value();
break;
}
}
clawArmPIDController.setPID(p, i, d);
int armPos = clawArm.getCurrentPosition();
double pid = clawArmPIDController.calculate(armPos, clawArm.getTargetPosition());
double ff = Math.cos(Math.toRadians(clawArm.getTargetPosition() / ticksInDegree)) * f;
double power = pid + ff;
clawArm.setPower(power);
}
}

7
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java
View File

@@ -4,12 +4,17 @@ 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.ClawArmTask;
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(false)));
RedCrabMinibot robot = new RedCrabMinibot(false);
addTask(new ClawArmTask(robot));
addState(new Pilot(robot));
}
}

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

@@ -1,5 +1,6 @@
package dev.cyberarm.minibots.red_crab.states;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.Range;
@@ -25,15 +26,16 @@ public class ClawArmMove extends CyberarmState {
@Override
public void start() {
robot.clawArm.setPositionTolerance(Utilities.motorAngle(motorTicks, gearRatio, toleranceAngle));
robot.clawArm.setTargetPositionTolerance(Utilities.motorAngle(motorTicks, gearRatio, toleranceAngle));
robot.clawArm.setTargetPosition(Utilities.motorAngle(motorTicks, gearRatio, targetAngle));
engine.blackboardSet("clawArmPower", power);
}
@Override
public void exec() {
if (robot.clawArm.atTargetPosition() || runTime() >= timeoutMS) {
int tolerance = robot.clawArm.getTargetPositionTolerance();
int position = robot.clawArm.getCurrentPosition();
if (Utilities.isBetween(position, position - tolerance, position + tolerance) || runTime() >= timeoutMS) {
this.finished();
}
}
@@ -41,7 +43,7 @@ public class ClawArmMove extends CyberarmState {
@Override
public void telemetry() {
engine.telemetry.addLine();
engine.telemetry.addData("Motor Power", robot.clawArm.get());
engine.telemetry.addData("Motor Power", robot.clawArm.getPower());
engine.telemetry.addData("Motor Position", robot.clawArm.getCurrentPosition());
engine.telemetry.addData("Motor Angle", Utilities.motorAngle(motorTicks, gearRatio, robot.clawArm.getCurrentPosition()));
engine.telemetry.addData("Motor Target Position", Utilities.motorAngle(motorTicks, gearRatio, targetAngle));

4
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmTask.java
View File

@@ -11,8 +11,6 @@ public class ClawArmTask extends CyberarmState {
@Override
public void exec() {
double power = engine.blackboardGetDouble("clawArmPower");
robot.clawArm.set(power);
robot.controlClawArm();
}
}

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

@@ -69,6 +69,9 @@ public class Pilot extends CyberarmState {
case "dpad_right":
clawArmPosition = RedCrabMinibot.ClawArm_COLLECT_FLOAT;
break;
case "start":
robot.reloadConfig();
break;
}
}
}
@@ -164,9 +167,9 @@ public class Pilot extends CyberarmState {
RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO,
RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE) - 25.0) {
robot.clawArm.set(0);
} else {
robot.clawArm.set(RedCrabMinibot.CLAW_ARM_MAX_SPEED);
// robot.clawArm.setPower(0);
// } else {
// robot.clawArm.setPower(RedCrabMinibot.CLAW_ARM_MAX_SPEED);
}
}