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: Hopefully fix ticks to distance calculations for drivetrain, fixed drivetrain gear ratio (see: https://docs.revrobotics.com/ultraplanetary/ultraplanetary-gearbox/cartridge-details#actual-cartridge-gear-ratios), changed claw arm to RUN_WITHOUT_ENCODERS to disable in-built pid controller (might be conflicting with FTCLib's that I'm using), using bulk caching of hub values, misc. changes

Browse Source
This commit is contained in:
Cyberarm
2023-12-20 10:37:34 -06:00
parent 8a3775af34
commit d2200fcc0a
10 changed files with 89 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,10 +1,10 @@
package dev.cyberarm.minibots.red_crab;
import com.arcrobotics.ftclib.controller.PIDController;
import com.arcrobotics.ftclib.controller.PIDFController;
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.lynx.LynxModule;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
@@ -15,7 +15,9 @@ import com.qualcomm.robotcore.util.Range;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
import org.firstinspires.ftc.vision.tfod.TfodProcessor;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
@@ -34,10 +36,16 @@ public class RedCrabMinibot {
/// TUNING CONSTANTS ///
public static double DRIVETRAIN_MAX_SPEED = 0.5;
public static double DRIVETRAIN_GEAR_RATIO = 13.0321;
public static int DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION = 28;
public static double DRIVETRAIN_WHEEL_DIAMETER_MM = 90.0;
public static double CLAW_ARM_MAX_SPEED = 0.5;
public static double CLAW_ARM_VELOCITY_DEGREES = 10;
public static double CLAW_ARM_kP = 0.1;
public static double CLAW_ARM_MAX_VELOCITY_DEGREES = 10;
public static double CLAW_ARM_kP = 0.0;
public static double CLAW_ARM_kI = 0.0;
public static double CLAW_ARM_kD = 0.0;
public static double CLAW_ARM_kF = 0.0;
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
@@ -95,6 +103,8 @@ public class RedCrabMinibot {
public TeamPropVisionProcessor teamProp = null;
/// Spike Mark detector: using OpenCV for full frame saturation threshold detection.
public SpikeMarkDetectorVisionProcessor spikeMark = null;
/// April Tag detector: Untested
public AprilTagProcessor aprilTag = null;
/// Doohickey
public VisionPortal visionPortal = null;
@@ -148,12 +158,12 @@ public class RedCrabMinibot {
right = new MotorGroup(frontRight, backRight);
/// --- MOTOR DISTANCE PER TICK
double gearRatio = config.variable("Robot", "Drivetrain_Tuning", "gear_ratio").value();
double motorTicks = config.variable("Robot", "Drivetrain_Tuning", "motor_ticks").value();
double wheelDiameterMM = config.variable("Robot", "Drivetrain_Tuning", "wheel_diameter_mm").value();
double wheelCircumference = Math.PI * wheelDiameterMM;
double distancePerTick = (motorTicks * gearRatio) / wheelCircumference; // raw motor encoder * gear ratio
double distancePerTick = Utilities.ticksToUnit(
DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION,
DRIVETRAIN_GEAR_RATIO,
DRIVETRAIN_WHEEL_DIAMETER_MM,
DistanceUnit.MM,
1);
frontLeft.setDistancePerPulse(distancePerTick);
frontRight.setDistancePerPulse(distancePerTick);
@@ -188,7 +198,7 @@ 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_USING_ENCODER);
clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
clawArm.setTargetPositionTolerance(CLAW_ARM_POSITION_TOLERANCE);
clawArm.setTargetPosition(0);
@@ -204,14 +214,17 @@ public class RedCrabMinibot {
rightClaw.setPosition((CLAW_RIGHT_CLOSED_POSITION));
/// DRONE LATCH ///
droneLatch = engine.hardwareMap.servo.get("droneLatch");
droneLatch = engine.hardwareMap.servo.get("droneLatch"); // | Ctrl|Ex Hub, Port: ??
droneLatch.setDirection(Servo.Direction.FORWARD);
droneLatch.setPosition(DRONE_LATCH_INITIAL_POSITION);
/// HOOK ARM ///
hookArm = engine.hardwareMap.servo.get("hookArm");
hookArm = engine.hardwareMap.servo.get("hookArm"); // | Ctrl|Ex Hub, Port: ??
hookArm.setDirection(Servo.Direction.FORWARD);
// hookArm.setPosition(HOOK_ARM_STOW_POSITION); // LEAVE OFF:
// Bulk read from hubs
Utilities.hubsBulkReadMode(engine.hardwareMap, LynxModule.BulkCachingMode.MANUAL);
}
public void reloadConfig() {
@@ -223,10 +236,13 @@ public class RedCrabMinibot {
private void loadConstants() {
/// Drivetrain
RedCrabMinibot.DRIVETRAIN_MAX_SPEED = config.variable("Robot", "Drivetrain_Tuning", "max_speed").value();
RedCrabMinibot.DRIVETRAIN_GEAR_RATIO = config.variable("Robot", "Drivetrain_Tuning", "gear_ratio").value();
RedCrabMinibot.DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION = config.variable("Robot", "Drivetrain_Tuning", "motor_ticks").value();
RedCrabMinibot.DRIVETRAIN_WHEEL_DIAMETER_MM = config.variable("Robot", "Drivetrain_Tuning", "wheel_diameter_mm").value();
/// CLAW ARM
RedCrabMinibot.CLAW_ARM_MAX_SPEED = config.variable("Robot", "ClawArm_Tuning", "max_speed").value();
RedCrabMinibot.CLAW_ARM_VELOCITY_DEGREES = config.variable("Robot", "ClawArm_Tuning", "velocityDEGREES").value();
RedCrabMinibot.CLAW_ARM_MAX_VELOCITY_DEGREES = config.variable("Robot", "ClawArm_Tuning", "max_velocityDEGREES").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();
@@ -303,7 +319,17 @@ public class RedCrabMinibot {
clawArm.getCurrent(CurrentUnit.MILLIAMPS),
clawArm.getCurrentPosition(),
clawArm.getVelocity());
engine.telemetry.addData("PIDF", "P: %.8f, I: %.8f, D: %.8f, F: %.8f", clawArmPIDFController.getP(), clawArmPIDFController.getI(), clawArmPIDFController.getD(), clawArmPIDFController.getF());
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());
engine.telemetry.addLine();
engine.telemetry.addLine("Servos");
@@ -324,34 +350,34 @@ public class RedCrabMinibot {
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);
double ticksInDegree = Utilities.motorAngleToTicks(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();
case "kP": // Proportional
CLAW_ARM_kP = v.value();
break;
case "kI":
i = v.value();
case "kI": // Integral
CLAW_ARM_kI = v.value();
break;
case "kD":
d = v.value();
case "kD": // Derivative (Dampener)
CLAW_ARM_kD = v.value();
break;
case "kF": // feedback
f = v.value();
case "kF": // Feedforward
CLAW_ARM_kF = v.value();
break;
}
}
clawArmPIDFController.setPIDF(p, i, d, f);
int armPos = clawArm.getCurrentPosition();
double pid = clawArmPIDFController.calculate(armPos, clawArm.getTargetPosition());
clawArmPIDFController.setTolerance(CLAW_ARM_POSITION_TOLERANCE);
clawArmPIDFController.setPIDF(CLAW_ARM_kP, CLAW_ARM_kI, CLAW_ARM_kD, CLAW_ARM_kF);
double pidf = clawArmPIDFController.calculate(armPos, clawArm.getTargetPosition());
// double ff = Math.cos(Math.toRadians(clawArm.getTargetPosition() / ticksInDegree)) * f;
double power = Range.clip(pid, -CLAW_ARM_MAX_SPEED, CLAW_ARM_MAX_SPEED);
// Limit pidf's value to max power range
double power = Range.clip(pidf, -CLAW_ARM_MAX_SPEED, CLAW_ARM_MAX_SPEED);
clawArm.setVelocityPIDFCoefficients(p, i, d, f);
clawArm.setVelocity(CLAW_ARM_VELOCITY_DEGREES, AngleUnit.DEGREES);
clawArm.setPower(power);
}
}

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

2
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java
View File

@@ -9,7 +9,7 @@ import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
@Autonomous(name = "Cyberarm Red Crab BLUE AUDIENCE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
public class RedCrabAutonomousBlueAudienceEngine extends CyberarmEngine {
public class RedCrabAutonomousBlueAudienceEngine extends RedCrabAutonomousEngine {
@Override
public void setup() {
RedCrabMinibot robot = new RedCrabMinibot(true);

2
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java
View File

@@ -9,7 +9,7 @@ import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
@Autonomous(name = "Cyberarm Red Crab BLUE BACKSTAGE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
public class RedCrabAutonomousBlueBackstageEngine extends CyberarmEngine {
public class RedCrabAutonomousBlueBackstageEngine extends RedCrabAutonomousEngine {
@Override
public void setup() {
RedCrabMinibot robot = new RedCrabMinibot(true);

13
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousEngine.java
View File

@@ -1,2 +1,13 @@
package dev.cyberarm.minibots.red_crab.engines;public class RedCrabAutonomousEngine {
package dev.cyberarm.minibots.red_crab.engines;
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.engine.V2.Utilities;
public abstract class RedCrabAutonomousEngine extends CyberarmEngine {
@Override
public void loop() {
Utilities.hubsClearBulkReadCache(hardwareMap);
super.loop();
}
}

2
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java
View File

@@ -9,7 +9,7 @@ import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
@Autonomous(name = "Cyberarm Red Crab RED AUDIENCE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
public class RedCrabAutonomousRedAudienceEngine extends CyberarmEngine {
public class RedCrabAutonomousRedAudienceEngine extends RedCrabAutonomousEngine {
@Override
public void setup() {
RedCrabMinibot robot = new RedCrabMinibot(true);

2
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java
View File

@@ -9,7 +9,7 @@ import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
@Autonomous(name = "Cyberarm Red Crab RED BACKSTAGE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
public class RedCrabAutonomousRedBackstageEngine extends CyberarmEngine {
public class RedCrabAutonomousRedBackstageEngine extends RedCrabAutonomousEngine {
@Override
public void setup() {
RedCrabMinibot robot = new RedCrabMinibot(true);

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

@@ -3,6 +3,7 @@ package dev.cyberarm.minibots.red_crab.engines;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.engine.V2.Utilities;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
import dev.cyberarm.minibots.red_crab.states.Pilot;
@@ -17,4 +18,11 @@ public class RedCrabTeleOpEngine extends CyberarmEngine {
addState(new Pilot(robot));
}
@Override
public void loop() {
Utilities.hubsClearBulkReadCache(hardwareMap);
super.loop();
}
}

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

@@ -1,9 +1,5 @@
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;
import dev.cyberarm.engine.V2.CyberarmState;
import dev.cyberarm.engine.V2.Utilities;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
@@ -26,8 +22,8 @@ public class ClawArmMove extends CyberarmState {
@Override
public void start() {
robot.clawArm.setTargetPositionTolerance(Utilities.motorAngle(motorTicks, gearRatio, toleranceAngle));
robot.clawArm.setTargetPosition(Utilities.motorAngle(motorTicks, gearRatio, targetAngle));
robot.clawArm.setTargetPositionTolerance(Utilities.motorAngleToTicks(motorTicks, gearRatio, toleranceAngle));
robot.clawArm.setTargetPosition(Utilities.motorAngleToTicks(motorTicks, gearRatio, targetAngle));
}
@Override
@@ -45,8 +41,8 @@ public class ClawArmMove extends CyberarmState {
engine.telemetry.addLine();
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));
engine.telemetry.addData("Motor Angle", Utilities.motorAngleToTicks(motorTicks, gearRatio, robot.clawArm.getCurrentPosition()));
engine.telemetry.addData("Motor Target Position", Utilities.motorAngleToTicks(motorTicks, gearRatio, targetAngle));
engine.telemetry.addData("Motor Target Angle", targetAngle);
engine.telemetry.addData("Timeout MS", timeoutMS);
progressBar(20, runTime() / timeoutMS);

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

@@ -128,7 +128,7 @@ public class Pilot extends CyberarmState {
private void clawArmAndWristController() {
switch (clawArmPosition) {
case RedCrabMinibot.ClawArm_STOW:
robot.clawArm.setTargetPosition(Utilities.motorAngle(
robot.clawArm.setTargetPosition(Utilities.motorAngleToTicks(
RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO,
RedCrabMinibot.CLAW_ARM_STOW_ANGLE));
@@ -136,7 +136,7 @@ public class Pilot extends CyberarmState {
robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_STOW_POSITION);
break;
case RedCrabMinibot.ClawArm_DEPOSIT:
robot.clawArm.setTargetPosition(Utilities.motorAngle(
robot.clawArm.setTargetPosition(Utilities.motorAngleToTicks(
RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO,
RedCrabMinibot.CLAW_ARM_DEPOSIT_ANGLE));
@@ -144,7 +144,7 @@ public class Pilot extends CyberarmState {
robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_DEPOSIT_POSITION);
break;
case RedCrabMinibot.ClawArm_COLLECT_FLOAT:
robot.clawArm.setTargetPosition(Utilities.motorAngle(
robot.clawArm.setTargetPosition(Utilities.motorAngleToTicks(
RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO,
RedCrabMinibot.CLAW_ARM_COLLECT_FLOAT_ANGLE));
@@ -152,7 +152,7 @@ public class Pilot extends CyberarmState {
robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_COLLECT_FLOAT_POSITION);
break;
case RedCrabMinibot.ClawArm_COLLECT:
robot.clawArm.setTargetPosition(Utilities.motorAngle(
robot.clawArm.setTargetPosition(Utilities.motorAngleToTicks(
RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO,
RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE));
@@ -163,7 +163,7 @@ public class Pilot extends CyberarmState {
}
if (clawArmPosition == RedCrabMinibot.ClawArm_COLLECT &&
robot.clawArm.getCurrentPosition() >= Utilities.motorAngle(
robot.clawArm.getCurrentPosition() >= Utilities.motorAngleToTicks(
RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO,
RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE) - 25.0) {