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

Disabled concept teleop I missed, added low battery voltage as a hardware fault, initial attempt at manually controlling velocity for arm (currently broken), misc. tweaks.

Browse Source
This commit is contained in:
Cyberarm
2023-01-23 17:37:25 -06:00
parent c63b7717b3
commit 6864defa6b
6 changed files with 167 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

144
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java
View File

@@ -1,5 +1,8 @@
package org.timecrafters.minibots.cyberarm.chiron;
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;
@@ -22,9 +25,12 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Var
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 = "CHIRON | Robot";
public final DcMotorEx backLeftDrive, frontRightDrive, frontLeftDrive, backRightDrive, arm;
public final ServoImplEx gripper, wrist;
public final IMU imu;
@@ -37,7 +43,9 @@ public class Robot {
public boolean hardwareFault = false;
private Status status = Status.OKAY, lastStatus = Status.OKAY;
private ArrayList<Status> reportedStatuses = new ArrayList<>();
private final CopyOnWriteArrayList<Status> reportedStatuses = new CopyOnWriteArrayList<>();
private final ConcurrentHashMap<String, Double> motorVelocityError = new ConcurrentHashMap<>();
private final ConcurrentHashMap<String, Double> motorVelocityLastTiming = new ConcurrentHashMap<>();
public enum ArmPosition {
COLLECT,
@@ -213,6 +221,32 @@ public class Robot {
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 in)", arm.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, 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));
@@ -241,32 +275,6 @@ public class Robot {
engine.telemetry.addLine();
// Motor Target Positions
engine.telemetry.addLine("Motor Target Positions");
engine.telemetry.addData(" Front Left Drive", frontLeftDrive.getTargetPosition());
engine.telemetry.addData(" Front Right Drive", frontRightDrive.getTargetPosition());
engine.telemetry.addData(" Back Left Drive", backLeftDrive.getTargetPosition());
engine.telemetry.addData(" Back Right Drive", backRightDrive.getTargetPosition());
engine.telemetry.addLine();
engine.telemetry.addData(" Arm", arm.getTargetPosition());
// Motor Velocity
engine.telemetry.addLine("Motor Velocity");
engine.telemetry.addData(" Front Left Drive", frontLeftDrive.getVelocity());
engine.telemetry.addData(" Front Right Drive", frontRightDrive.getVelocity());
engine.telemetry.addData(" Back Left Drive", backLeftDrive.getVelocity());
engine.telemetry.addData(" Back Right Drive", backRightDrive.getVelocity());
engine.telemetry.addLine();
engine.telemetry.addData(" Arm", arm.getVelocity());
engine.telemetry.addLine();
// Servos
engine.telemetry.addLine("Servos");
engine.telemetry.addData(" Gripper Direction", gripper.getDirection());
@@ -300,6 +308,17 @@ public class Robot {
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<>();
@@ -345,6 +364,11 @@ public class Robot {
hub.clearBulkCache();
}
if (getVoltage() < 9.75) {
reportStatus(Status.DANGER);
hardwareFault = true;
}
status = Status.OKAY;
for (Status s : reportedStatuses) {
if (s.ordinal() > status.ordinal()) {
@@ -368,6 +392,12 @@ public class Robot {
automaticLEDStatus();
}
public void stop() {
if (expansionHub != null) {
expansionHub.setPattern(ledPatternStandby());
}
}
private void automaticLEDStatus() {
switch (status) {
case OKAY:
@@ -474,8 +504,66 @@ public class Robot {
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.");
}
public double getVoltage() {
return engine.hardwareMap.voltageSensor.iterator().next().getVoltage();
}
public double facing() {
double imuDegrees = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
return (imuDegrees + imuAngleOffset + 360.0) % 360.0;
}

7
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousEngine.java
View File

@@ -50,4 +50,11 @@ public class AutonomousEngine extends CyberarmEngine {
robot.standardTelemetry();
}
@Override
public void stop() {
robot.stop();
super.stop();
}
}

9
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/ConfigureAndTestHardwareEngine.java
View File

@@ -38,11 +38,20 @@ public class ConfigureAndTestHardwareEngine extends CyberarmEngine {
@Override
public void loop() {
robot.update();
super.loop();
robot.standardTelemetry();
}
@Override
public void stop() {
robot.stop();
super.stop();
}
private class MotorSetupState extends CyberarmState {
private final DcMotorEx motor;
private final String name, direction_key;

9
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/TeleOpEngine.java
View File

@@ -27,8 +27,17 @@ public class TeleOpEngine extends CyberarmEngine {
@Override
public void loop() {
robot.update();
super.loop();
robot.standardTelemetry();
}
@Override
public void stop() {
robot.stop();
super.stop();
}
}

29
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java
View File

@@ -1,9 +1,16 @@
package org.timecrafters.minibots.cyberarm.chiron.states.teleop;
import android.annotation.SuppressLint;
import android.os.Build;
import androidx.annotation.RequiresApi;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.Position;
import org.timecrafters.minibots.cyberarm.chiron.Robot;
public class ArmDriverControl extends CyberarmState {
@@ -88,10 +95,17 @@ public class ArmDriverControl extends CyberarmState {
private void stopArm() {
robot.arm.setVelocity(0);
robot.arm.setPower(0);
// RUN_TO_POSITION seems to override power we request
robot.arm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}
private void automatics() {
automaticWrist();
if (!robot.hardwareFault) {
automaticWrist();
automaticArmVelocity();
}
automaticHardwareMonitor();
}
@@ -110,14 +124,21 @@ public class ArmDriverControl extends CyberarmState {
}
}
private void automaticArmVelocity() {
robot.arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.controlMotorPIDF(
robot.arm,
"Arm",
robot.angleToTicks(robot.tuningConfig("arm_velocity_in_degrees_per_second").value()),
12.0 / robot.getVoltage());
}
private void automaticHardwareMonitor() {
if (robot.hardwareFault) {
robot.reportStatus(Robot.Status.DANGER);
stopArm();
} else {
robot.arm.setVelocity(
robot.angleToTicks(robot.tuningConfig("arm_velocity_in_degrees_per_second").value()));
}
}