TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-13 09:42:34 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added FTCLib, implemented Arm PID(f) controller

Browse Source
This commit is contained in:
Cyberarm
2023-03-04 12:42:43 -06:00
parent d59e7a54f7
commit fa54f5f209
5 changed files with 755 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

625
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/Robot.java Normal file
View File

@@ -0,0 +1,625 @@
package org.timecrafters.minibots.cyberarm.phoenix;
import android.annotation.SuppressLint;
import android.util.Log;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.Blinker;
import com.qualcomm.robotcore.hardware.CRServoImplEx;
import com.qualcomm.robotcore.hardware.ColorSensor;
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;
import com.qualcomm.robotcore.hardware.ServoImplEx;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.firstinspires.ftc.robotcore.external.ClassFactory;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
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.robotcore.external.navigation.VuforiaLocalizer;
import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
import org.timecrafters.minibots.cyberarm.chiron.tasks.FieldLocalizer;
import java.util.ArrayList;
import java.util.concurrent.ConcurrentHashMap;
import java.util.concurrent.CopyOnWriteArrayList;
import java.util.concurrent.TimeUnit;
public class Robot {
private static final String TAG = "Phoenix | Robot";
public final DcMotorEx backLeftDrive, frontRightDrive, frontLeftDrive, backRightDrive, arm;
// public final ServoImplEx gripper;
public final IMU imu;
public LynxModule expansionHub;
public final double imuAngleOffset, initialFacing;
public boolean armManuallyControlled = false;
public boolean automaticAntiTipActive = false;
public boolean hardwareFault = false;
public String hardwareFaultMessage = "";
private Status status = Status.OKAY, lastStatus = Status.OKAY;
private final CopyOnWriteArrayList<Status> reportedStatuses = new CopyOnWriteArrayList<>();
private final ConcurrentHashMap<String, Double> motorVelocityError = new ConcurrentHashMap<>();
private final ConcurrentHashMap<String, Double> motorVelocityLastTiming = new ConcurrentHashMap<>();
private final ConcurrentHashMap<String, Double> motorTargetVelocity = new ConcurrentHashMap<>();
public enum ArmPosition {
COLLECT,
GROUND,
LOW,
MEDIUM,
HIGH
}
public enum Status {
OKAY,
MONITORING,
WARNING,
DANGER
}
private final CyberarmEngine engine;
private TimeCraftersConfiguration configuration;
private final double radius, diameter;
private final double wheelRadius, wheelGearRatio, armGearRatio;
private final int wheelTicksPerRevolution, armTicksPerRevolution;
private final VuforiaLocalizer vuforia;
private final TFObjectDetector tfod;
private boolean LEDStatusToggle = false;
private double lastLEDStatusAnimationTime = 0;
public Robot(CyberarmEngine engine, TimeCraftersConfiguration configuration) {
this.engine = engine;
this.configuration = configuration;
radius = tuningConfig("field_localizer_robot_radius").value();
diameter = radius * 2;
imuAngleOffset = hardwareConfig("imu_angle_offset").value();
wheelRadius = tuningConfig("wheel_radius").value();
wheelGearRatio = tuningConfig("wheel_gear_ratio").value();
wheelTicksPerRevolution = tuningConfig("wheel_ticks_per_revolution").value();
armGearRatio = tuningConfig("arm_gear_ratio").value();
armTicksPerRevolution = tuningConfig("arm_ticks_per_revolution").value();
// FIXME: Rename motors in configuration
// Define hardware
backLeftDrive = engine.hardwareMap.get(DcMotorEx.class, "Back Left"); // MOTOR PORT: 2 - CONTROL HUB
frontRightDrive = engine.hardwareMap.get(DcMotorEx.class, "Front Right"); // MOTOR PORT: 1 - CONTROL HUB
frontLeftDrive = engine.hardwareMap.get(DcMotorEx.class, "Front Left"); // MOTOR PORT: 3 - CONTROL HUB
backRightDrive = engine.hardwareMap.get(DcMotorEx.class, "Back Right"); // MOTOR PORT: 0 - CONTROL HUB
// FIXME: Rename lift_drive to arm in hardware config
arm = engine.hardwareMap.get(DcMotorEx.class, "ArmMotor"); // MOTOR PORT: 2 - EXPANSION HUB
// gripper = engine.hardwareMap.get(ServoImplEx.class, "gripper"); // SERVO PORT: ?
imu = engine.hardwareMap.get(IMU.class, "imu");
// Configure hardware
// MOTORS
// DIRECTION
frontLeftDrive.setDirection(hardwareConfig("front_left_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
frontRightDrive.setDirection(hardwareConfig("front_right_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
backLeftDrive.setDirection(hardwareConfig("back_left_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
backRightDrive.setDirection(hardwareConfig("back_right_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
arm.setDirection(hardwareConfig("arm_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
// RUNMODE
frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
arm.setTargetPosition(0);
arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// ZERO POWER BEHAVIOR
frontLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// MOTOR POWER
arm.setPower(tuningConfig("arm_automatic_power").value());
// IMU
IMU.Parameters parameters = new IMU.Parameters(
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP,
RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD));
imu.initialize(parameters);
// Preserve our "initial" facing, since we transform it from zero.
initialFacing = facing();
// BulkRead from Hubs
for (LynxModule hub : engine.hardwareMap.getAll(LynxModule.class)) {
hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
if (!hub.isParent() && expansionHub == null) {
expansionHub = hub;
}
}
// Set LED pattern
if (expansionHub != null) {
expansionHub.setPattern(ledPatternOkay());
}
// Webcam
vuforia = initVuforia();
tfod = initTfod();
// Drive Encoder Error Setup
engine.blackboardSet("left_drive_error", 0);
engine.blackboardSet("right_drive_error", 0);
}
private VuforiaLocalizer initVuforia() {
VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
parameters.vuforiaLicenseKey = hardwareConfig("vuforia_license_key").value();
parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1");
return ClassFactory.getInstance().createVuforia(parameters);
}
private TFObjectDetector initTfod() {
int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier(
"tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
tfodParameters.minResultConfidence = 0.75f;
tfodParameters.isModelTensorFlow2 = true;
tfodParameters.inputSize = 300;
TFObjectDetector tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
tfod.loadModelFromAsset("PowerPlay.tflite", "1 Bolt", "2 Bulb", "3 Panel");
return tfod;
}
public void reloadConfig() {
String name = configuration.getConfig().getName();
configuration = new TimeCraftersConfiguration(name);
}
public void standardTelemetry() {
engine.telemetry.addLine();
// STATUS
engine.telemetry.addLine("DATA");
engine.telemetry.addData(" Robot Status", status);
engine.telemetry.addData(" Hardware Fault", hardwareFault);
engine.telemetry.addData(" Hardware Fault Message", hardwareFaultMessage);
engine.telemetry.addLine();
// Motor Powers
engine.telemetry.addLine("Motor Powers");
engine.telemetry.addData(" Front Left Drive", frontLeftDrive.getPower());
engine.telemetry.addData(" Front Right Drive", frontRightDrive.getPower());
engine.telemetry.addData(" Back Left Drive", backLeftDrive.getPower());
engine.telemetry.addData(" Back Right Drive", backRightDrive.getPower());
engine.telemetry.addLine();
engine.telemetry.addData(" Arm", arm.getPower());
engine.telemetry.addLine();
// Motor Positions
engine.telemetry.addLine("Motor Positions");
engine.telemetry.addData(" Front Left Drive", "%d (%8.2f in)", frontLeftDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, frontLeftDrive.getCurrentPosition()));
engine.telemetry.addData(" Front Right Drive", "%d (%8.2f in)", frontRightDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, frontRightDrive.getCurrentPosition()));
engine.telemetry.addData(" Back Left Drive", "%d (%8.2f in)", backLeftDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, backLeftDrive.getCurrentPosition()));
engine.telemetry.addData(" Back Right Drive", "%d (%8.2f in)", backRightDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, backRightDrive.getCurrentPosition()));
engine.telemetry.addLine();
engine.telemetry.addData(" Arm", "%d (%8.2f degrees)", arm.getCurrentPosition(), ticksToAngle(arm.getCurrentPosition()));
engine.telemetry.addLine();
// Motor Target Positions
engine.telemetry.addLine("Motor Target Positions");
engine.telemetry.addData(" Front Left Drive", "%d (%8.2f in)", frontLeftDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, frontLeftDrive.getTargetPosition()));
engine.telemetry.addData(" Front Right Drive", "%d (%8.2f in)", frontRightDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, frontRightDrive.getTargetPosition()));
engine.telemetry.addData(" Back Left Drive", "%d (%8.2f in)", backLeftDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, backLeftDrive.getTargetPosition()));
engine.telemetry.addData(" Back Right Drive", "%d (%8.2f in)", backRightDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, backRightDrive.getTargetPosition()));
engine.telemetry.addLine();
engine.telemetry.addData(" Arm", "%d (%8.2f degrees)", arm.getTargetPosition(), ticksToAngle(arm.getTargetPosition()));
// Motor Velocity
engine.telemetry.addLine("Motor Velocity");
engine.telemetry.addData(" Front Left Drive", "%8.2f (%8.2f in)", frontLeftDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) frontLeftDrive.getVelocity()));
engine.telemetry.addData(" Front Right Drive", "%8.2f (%8.2f in)", frontRightDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) frontRightDrive.getVelocity()));
engine.telemetry.addData(" Back Left Drive", "%8.2f (%8.2f in)", backLeftDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) backLeftDrive.getVelocity()));
engine.telemetry.addData(" Back Right Drive", "%8.2f (%8.2f in)", backRightDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) backRightDrive.getVelocity()));
engine.telemetry.addLine();
engine.telemetry.addData(" Arm", "%8.2f (%8.2f degrees)", arm.getVelocity(), ticksToAngle((int)arm.getVelocity()));
engine.telemetry.addLine();
// Motor Currents
engine.telemetry.addLine("Motor Currents (AMPS)");
engine.telemetry.addData(" Front Left Drive", frontLeftDrive.getCurrent(CurrentUnit.AMPS));
engine.telemetry.addData(" Front Right Drive", frontRightDrive.getCurrent(CurrentUnit.AMPS));
engine.telemetry.addData(" Back Left Drive", backLeftDrive.getCurrent(CurrentUnit.AMPS));
engine.telemetry.addData(" Back Right Drive", backRightDrive.getCurrent(CurrentUnit.AMPS));
engine.telemetry.addLine();
engine.telemetry.addData(" Arm", arm.getCurrent(CurrentUnit.AMPS));
engine.telemetry.addLine();
// Motor Directions
engine.telemetry.addLine("Motor Directions");
engine.telemetry.addData(" Front Left Drive", frontLeftDrive.getDirection());
engine.telemetry.addData(" Front Right Drive", frontRightDrive.getDirection());
engine.telemetry.addData(" Back Left Drive", backLeftDrive.getDirection());
engine.telemetry.addData(" Back Right Drive", backRightDrive.getDirection());
engine.telemetry.addLine();
engine.telemetry.addData(" Arm", arm.getDirection());
engine.telemetry.addLine();
// Sensors / IMU
engine.telemetry.addLine("IMU");
engine.telemetry.addData(" Facing (Degrees)", facing());
engine.telemetry.addData(" Heading (Radians)", heading());
engine.telemetry.addData(" Turn Rate", turnRate());
engine.telemetry.addData(" Angle Offset (Degrees)", imuAngleOffset);
engine.telemetry.addLine();
}
private ArrayList<Blinker.Step> ledPatternStandby() {
final ArrayList<Blinker.Step> steps = new ArrayList<>();
steps.add(new Blinker.Step(0x008000, 750, TimeUnit.MILLISECONDS));
steps.add(new Blinker.Step(0x005000, 750, TimeUnit.MILLISECONDS));
steps.add(new Blinker.Step(0x002000, 750, TimeUnit.MILLISECONDS));
steps.add(new Blinker.Step(0x001000, 250, TimeUnit.MILLISECONDS));
return steps;
}
private ArrayList<Blinker.Step> ledPatternOkay() {
final ArrayList<Blinker.Step> steps = new ArrayList<>();
steps.add(new Blinker.Step(0x00aa00, 500, TimeUnit.MILLISECONDS));
steps.add(new Blinker.Step(0x00aa44, 500, TimeUnit.MILLISECONDS));
return steps;
}
private ArrayList<Blinker.Step> ledPatternMonitoring() {
final ArrayList<Blinker.Step> steps = new ArrayList<>();
steps.add(new Blinker.Step(0xaaaaaa, 500, TimeUnit.MILLISECONDS));
steps.add(new Blinker.Step(0x000000, 250, TimeUnit.MILLISECONDS));
return steps;
}
private ArrayList<Blinker.Step> ledPatternWarning() {
final ArrayList<Blinker.Step> steps = new ArrayList<>();
steps.add(new Blinker.Step(0xffff00, 500, TimeUnit.MILLISECONDS));
steps.add(new Blinker.Step(0xff8800, 500, TimeUnit.MILLISECONDS));
return steps;
}
private ArrayList<Blinker.Step> ledPatternDanger() {
final ArrayList<Blinker.Step> steps = new ArrayList<>();
steps.add(new Blinker.Step(0xff0000, 250, TimeUnit.MILLISECONDS));
steps.add(new Blinker.Step(0x000000, 100, TimeUnit.MILLISECONDS));
return steps;
}
public void reportStatus(Status status) {
reportedStatuses.add(status);
}
public void update() {
for (LynxModule hub : engine.hardwareMap.getAll(LynxModule.class)) {
hub.clearBulkCache();
}
double voltage = getVoltage();
if (voltage < 9.75) {
reportStatus(Status.DANGER);
hardwareFaultMessage = "Battery voltage to low! (" + voltage + " volts)";
hardwareFault = true;
}
status = Status.OKAY;
for (Status s : reportedStatuses) {
if (s.ordinal() > status.ordinal()) {
status = s;
}
}
reportedStatuses.clear();
if (status != lastStatus) {
lastStatus = status;
if (expansionHub != null) {
if (lastStatus == Status.OKAY) { expansionHub.setPattern(ledPatternOkay()); }
if (lastStatus == Status.MONITORING) { expansionHub.setPattern(ledPatternMonitoring()); }
if (lastStatus == Status.WARNING) { expansionHub.setPattern(ledPatternWarning()); }
if (lastStatus == Status.DANGER) { expansionHub.setPattern(ledPatternDanger()); }
}
}
}
public void stop() {
if (expansionHub != null) {
expansionHub.setPattern(ledPatternStandby());
}
}
public void armPosition(ArmPosition position) {
if (hardwareFault) {
return;
}
reportStatus(Status.WARNING);
switch (position) {
case COLLECT:
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_collect").value()));
break;
case GROUND:
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_ground").value()));
break;
case LOW:
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_low").value()));
break;
case MEDIUM:
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_medium").value()));
break;
case HIGH:
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_high").value()));
break;
default:
throw new RuntimeException("Unexpected arm position!");
}
}
// 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;
}
public Status getStatus() { return status; }
public double getRadius() { return radius; }
public double getDiameter() { return diameter; }
public double getVoltage() {
return engine.hardwareMap.voltageSensor.iterator().next().getVoltage();
}
public TFObjectDetector getTfod() { return tfod; }
public VuforiaLocalizer getVuforia() { return vuforia; }
public TimeCraftersConfiguration getConfiguration() { return configuration; }
// For: Drive Wheels
public int unitToTicks(DistanceUnit unit, double distance) {
double fI = (wheelGearRatio * wheelTicksPerRevolution) / (wheelRadius * 2 * Math.PI * (wheelGearRatio * wheelTicksPerRevolution) / (wheelGearRatio * wheelTicksPerRevolution));
double inches = unit.toInches(unit.fromUnit(unit, distance));
double ticks = fI * inches;
return (int)ticks;
}
// For: Drive Wheels
public double ticksToUnit(DistanceUnit unit, int ticks) {
// Convert to inches, then to unit.
double inches = wheelRadius * 2 * Math.PI * ticks / (wheelGearRatio * wheelTicksPerRevolution);
return unit.fromUnit(DistanceUnit.INCH, inches);
}
// For: Arm
public int angleToTicks(double angle) {
double d = (armGearRatio * armTicksPerRevolution) / 360.0;
// Casting to float so that the int version of Math.round is used.
return Math.round((float)d * (float)angle);
}
// For: Arm
public double ticksToAngle(int ticks) {
double oneDegree = 360.0 / (armGearRatio * armTicksPerRevolution);
return oneDegree * ticks;
}
public Variable hardwareConfig(String variableName) {
Action hardwareConfiguration = configuration.action("Robot", "Hardware");
for (Variable v : hardwareConfiguration.getVariables()) {
if (variableName.trim().equals(v.name)) {
return v;
}
}
throw new RuntimeException("Failed to find variable with name: " + variableName + " in group: Robot, action: Hardware");
}
public Variable tuningConfig(String variableName) {
Action action = configuration.action("Robot", "Tuning");
for (Variable v : action.getVariables()) {
if (variableName.trim().equals(v.name)) {
return v;
}
}
throw new RuntimeException("Failed to find variable with name: " + variableName + " in group: Robot, action: Tuning");
}
@SuppressLint("NewApi")
public void controlMotorPIDF(DcMotorEx motor, String motorName, double targetVelocity, double feedForward) {
Action action = configuration.action("Robot", "Tuning_PIDF_" + motorName);
double proportional = 0, integral = 0, derivative = 0;
for (Variable v : action.getVariables()) {
switch (v.name.trim()) {
case "proportional":
proportional = v.value();
break;
case "integral":
integral = v.value();
break;
case "derivative":
derivative = v.value();
break;
}
}
double interval = (engine.getRuntime() - motorVelocityLastTiming.getOrDefault(motorName, 0.0));
double distance = motor.getTargetPosition() - motor.getCurrentPosition();
if (Math.abs(distance) < Math.abs(targetVelocity)) {
if ((targetVelocity < 0 && distance > 0) || (targetVelocity > 0 && distance < 0)) {
targetVelocity = -distance;
} else {
targetVelocity = distance;
}
}
double error = targetVelocity - motor.getVelocity();
double deltaError = error - motorVelocityError.getOrDefault(motorName, 0.0);
double kIntegral = error * interval;
double kDerivative = deltaError / interval;
double kp = proportional * error;
double ki = integral * kIntegral;
double kd = derivative * kDerivative;
motorVelocityError.put(motorName, error);
motorVelocityLastTiming.put(motorName, engine.getRuntime());
double newTargetVelocity = kp + ki + kd + targetVelocity;
motor.setVelocity(newTargetVelocity);
Log.d(TAG, "Interval: " + interval + "ms, Error: " + error + " ticks, deltaError: " + deltaError + " ticks, distance: " +
distance + " ticks, kIntegral: " + kIntegral + ", kDerivative: " + kDerivative + ", proportional: " + proportional +
", integral: " + integral + ", derivative: " + derivative + ", kp: " + kp + ", ki: " + ki + ", kd: " + kd +
", targetVelocity: " + targetVelocity + " ticks, new Target Velocity: " + newTargetVelocity + " ticks, + motorVelocity: " + motor.getVelocity() + " ticks.");
}
@SuppressLint("NewApi")
public void controlArmMotor(double targetVelocity) {
// double time = System.currentTimeMillis();
// double newTargetVelocity = motorTargetVelocity.getOrDefault("Arm", targetVelocity);
// double lastTiming = motorVelocityLastTiming.getOrDefault("Arm", time);
// double deltaTime = (time - lastTiming) * 0.001;
//
// double distanceToTarget = arm.getTargetPosition() - arm.getCurrentPosition();
// double adjustedTargetVelocity = Math.abs(distanceToTarget) < targetVelocity ? Math.abs(distanceToTarget) : targetVelocity;
//
// double error = adjustedTargetVelocity - arm.getVelocity();
// double kp = 0.9;
//
// newTargetVelocity += error * kp * deltaTime;
//
// motorTargetVelocity.put("Arm", newTargetVelocity);
// motorVelocityLastTiming.put("Arm", time);
// arm.setVelocity(newTargetVelocity);
arm.setPower(tuningConfig("arm_automatic_power").value());
}
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;
}
}

47
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/engines/TeleOp.java Normal file
View File

@@ -0,0 +1,47 @@
package org.timecrafters.minibots.cyberarm.phoenix.engines;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import org.timecrafters.minibots.cyberarm.phoenix.Robot;
@com.qualcomm.robotcore.eventloop.opmode.TeleOp(name = "Mentor Phoenix | TeleOp", group = "Mentor Phoenix")
public class TeleOp extends CyberarmEngine {
private Robot robot;
private TimeCraftersConfiguration configuration;
final private String configurationName = "MentorPhoenix", actionsGroupName = "TeleOpStates";
@Override
public void setup() {
configuration = new TimeCraftersConfiguration(configurationName);
robot = new Robot(
this,
configuration
);
robot.imu.resetYaw();
setupFromConfig(
robot.getConfiguration(),
"org.timecrafters.minibots.cyberarm.phoenix.states.teleop",
robot,
Robot.class,
actionsGroupName);
}
@Override
public void loop() {
robot.update();
super.loop();
robot.standardTelemetry();
}
@Override
public void stop() {
robot.stop();
super.stop();
}
}

81
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/states/teleop/ArmController.java Normal file
View File

@@ -0,0 +1,81 @@
package org.timecrafters.minibots.cyberarm.phoenix.states.teleop;
import com.arcrobotics.ftclib.controller.PIDController;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
import org.timecrafters.minibots.cyberarm.phoenix.Robot;
public class ArmController extends CyberarmState {
private final Robot robot;
private final String groupName, actionName;
private PIDController pidController;
private double p = 0, i = 0, d = 0, f = 0;
private final double ticksInDegree = 700 / 180;
public ArmController(Robot robot, String groupName, String actionName) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
pidController = new PIDController(p, i, d);
}
@Override
public void exec() {
Action action = robot.getConfiguration().action("Robot", "Tuning_PIDF_Arm");
for (Variable v : action.getVariables()) {
switch (v.name.trim()) {
case "proportional":
p = v.value();
break;
case "integral":
i = v.value();
break;
case "derivative":
d = v.value();
break;
case "feed":
f = v.value();
break;
}
}
pidController.setPID(p, i, d);
int armPos = robot.arm.getCurrentPosition();
double pid = pidController.calculate(armPos, robot.arm.getTargetPosition());
double ff = Math.cos(Math.toRadians(robot.arm.getTargetPosition() / ticksInDegree)) * f;
double power = pid + ff;
robot.arm.setPower(power);
}
@Override
public void buttonDown(Gamepad gamepad, String button) {
if (gamepad != engine.gamepad2) {
return;
}
switch (button) {
case "guide":
robot.reloadConfig();
break;
case "a":
robot.armPosition(Robot.ArmPosition.COLLECT);
break;
case "x":
robot.armPosition(Robot.ArmPosition.GROUND);
break;
case "b":
robot.armPosition(Robot.ArmPosition.LOW);
break;
case "y":
robot.armPosition(Robot.ArmPosition.MEDIUM);
break;
}
}
}

2
build.common.gradle
View File

@@ -51,7 +51,7 @@ android {
defaultConfig {
signingConfig signingConfigs.debug
applicationId 'com.qualcomm.ftcrobotcontroller'
minSdkVersion 23
minSdkVersion 24
//noinspection ExpiredTargetSdkVersion
targetSdkVersion 28

1
build.dependencies.gradle
View File

@@ -22,5 +22,6 @@ dependencies {
implementation 'org.apache.commons:commons-math3:3.6.1'
implementation 'com.acmerobotics.roadrunner:core:0.5.6'
implementation 'com.acmerobotics.dashboard:dashboard:0.4.8'
implementation 'org.ftclib.ftclib:core:2.1.1'
}