SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Record signals on Java side

Browse Source
This commit is contained in:
Ryan Brott
2023-03-05 21:01:03 -08:00
parent 47151cbebd
commit 332704b536
3 changed files with 152 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

137
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/AngularRampLogger.java
View File

@ -1,7 +1,10 @@
package org.firstinspires.ftc.teamcode.tuning;
import com.qualcomm.hardware.lynx.LynxDcMotorController;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.util.SerialNumber;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
@ -10,69 +13,80 @@ import org.firstinspires.ftc.teamcode.util.MidpointTimer;
import java.util.ArrayList;
import java.util.List;
import java.util.Map;
public final class AngularRampLogger extends LinearOpMode {
private static double power(double seconds) {
return Math.min(0.1 * seconds, 0.9);
}
private static class Signal {
public final List<Double> times = new ArrayList<>();
public final List<Double> values = new ArrayList<>();
}
private static void recordEncoderData(Encoder e, Map<SerialNumber, Double> ts, Signal ps, Signal vs) {
SerialNumber sn = ((LynxDcMotorController) e.getController()).getSerialNumber();
Encoder.PositionVelocityPair p = e.getPositionAndVelocity();
ps.times.add(ts.get(sn));
ps.values.add((double) p.position);
vs.times.add(ts.get(sn));
vs.values.add((double) p.velocity);
}
@Override
public void runOpMode() throws InterruptedException {
DriveView view = new DriveView(hardwareMap);
view.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
class Data {
public final String type = view.type;
public final List<List<Double>> leftPowerTimes = new ArrayList<>();
public final List<List<Double>> leftPowers = new ArrayList<>();
public final List<Signal> leftPowers = new ArrayList<>();
public final List<Signal> rightPowers = new ArrayList<>();
public final List<List<Double>> rightPowerTimes = new ArrayList<>();
public final List<List<Double>> rightPowers = new ArrayList<>();
public final Signal voltages = new Signal();
public final List<Double> voltageTimes = new ArrayList<>();
public final List<Double> voltages = new ArrayList<>();
public final List<Signal> leftEncPositions = new ArrayList<>();
public final List<Signal> rightEncPositions = new ArrayList<>();
public final List<Signal> parEncPositions = new ArrayList<>();
public final List<Signal> perpEncPositions = new ArrayList<>();
public final List<Double> encTimes = new ArrayList<>();
public final List<List<Integer>> leftEncPositions = new ArrayList<>();
public final List<List<Integer>> leftEncVels = new ArrayList<>();
public final List<List<Integer>> rightEncPositions = new ArrayList<>();
public final List<List<Integer>> rightEncVels = new ArrayList<>();
public final List<List<Integer>> parEncPositions = new ArrayList<>();
public final List<List<Integer>> parEncVels = new ArrayList<>();
public final List<List<Integer>> perpEncPositions = new ArrayList<>();
public final List<List<Integer>> perpEncVels = new ArrayList<>();
public final List<Signal> leftEncVels = new ArrayList<>();
public final List<Signal> rightEncVels = new ArrayList<>();
public final List<Signal> parEncVels = new ArrayList<>();
public final List<Signal> perpEncVels = new ArrayList<>();
public final List<Double> angVelTimes = new ArrayList<>();
public final List<List<Double>> angVels = new ArrayList<>();
public final List<Signal> angVels = new ArrayList<>();
}
Data data = new Data();
for (DcMotorEx m : view.leftMotors) {
data.leftPowerTimes.add(new ArrayList<>());
data.leftPowers.add(new ArrayList<>());
data.leftPowers.add(new Signal());
}
for (DcMotorEx m : view.rightMotors) {
data.rightPowerTimes.add(new ArrayList<>());
data.rightPowers.add(new ArrayList<>());
data.rightPowers.add(new Signal());
}
for (Encoder e : view.leftEncs) {
data.leftEncPositions.add(new ArrayList<>());
data.leftEncVels.add(new ArrayList<>());
data.leftEncPositions.add(new Signal());
data.leftEncVels.add(new Signal());
}
for (Encoder e : view.rightEncs) {
data.rightEncPositions.add(new ArrayList<>());
data.rightEncVels.add(new ArrayList<>());
data.rightEncPositions.add(new Signal());
data.rightEncVels.add(new Signal());
}
for (Encoder e : view.parEncs) {
data.parEncPositions.add(new ArrayList<>());
data.parEncVels.add(new ArrayList<>());
data.parEncPositions.add(new Signal());
data.parEncVels.add(new Signal());
}
for (Encoder e : view.perpEncs) {
data.perpEncPositions.add(new ArrayList<>());
data.perpEncVels.add(new ArrayList<>());
data.perpEncPositions.add(new Signal());
data.perpEncVels.add(new Signal());
}
for (int i = 0; i < 3; i++) {
data.angVels.add(new ArrayList<>());
data.angVels.add(new Signal());
}
waitForStart();
@ -83,48 +97,65 @@ public final class AngularRampLogger extends LinearOpMode {
double power = -power(t.seconds());
view.leftMotors.get(i).setPower(power);
data.leftPowers.get(i).add(power);
data.leftPowerTimes.get(i).add(t.addSplit());
Signal s = data.leftPowers.get(i);
s.times.add(t.addSplit());
s.values.add(power);
}
for (int i = 0; i < view.rightMotors.size(); i++) {
double power = power(t.seconds());
view.rightMotors.get(i).setPower(power);
data.rightPowers.get(i).add(power);
data.rightPowerTimes.get(i).add(t.addSplit());
Signal s = data.rightPowers.get(i);
s.times.add(t.addSplit());
s.values.add(power);
}
data.voltages.add(view.voltageSensor.getVoltage());
data.voltageTimes.add(t.addSplit());
data.voltages.values.add(view.voltageSensor.getVoltage());
data.voltages.times.add(t.addSplit());
Map<SerialNumber, Double> encTimes = view.resetAndBulkRead(t);
for (int i = 0; i < view.leftEncs.size(); i++) {
Encoder.PositionVelocityPair p = view.leftEncs.get(i).getPositionAndVelocity();
data.leftEncPositions.get(i).add(p.position);
data.leftEncVels.get(i).add(p.velocity);
recordEncoderData(
view.leftEncs.get(i),
encTimes,
data.leftEncPositions.get(i),
data.leftEncVels.get(i)
);
}
for (int i = 0; i < view.rightEncs.size(); i++) {
Encoder.PositionVelocityPair p = view.rightEncs.get(i).getPositionAndVelocity();
data.rightEncPositions.get(i).add(p.position);
data.rightEncVels.get(i).add(p.velocity);
recordEncoderData(
view.rightEncs.get(i),
encTimes,
data.rightEncPositions.get(i),
data.rightEncVels.get(i)
);
}
for (int i = 0; i < view.parEncs.size(); i++) {
Encoder.PositionVelocityPair p = view.parEncs.get(i).getPositionAndVelocity();
data.parEncPositions.get(i).add(p.position);
data.parEncVels.get(i).add(p.velocity);
recordEncoderData(
view.parEncs.get(i),
encTimes,
data.parEncPositions.get(i),
data.parEncVels.get(i)
);
}
for (int i = 0; i < view.perpEncs.size(); i++) {
Encoder.PositionVelocityPair p = view.perpEncs.get(i).getPositionAndVelocity();
data.perpEncPositions.get(i).add(p.position);
data.perpEncVels.get(i).add(p.velocity);
recordEncoderData(
view.perpEncs.get(i),
encTimes,
data.perpEncPositions.get(i),
data.perpEncVels.get(i)
);
}
data.encTimes.add(t.addSplit());
AngularVelocity av = view.imu.getRobotAngularVelocity(AngleUnit.RADIANS);
data.angVels.get(0).add((double) av.xRotationRate);
data.angVels.get(1).add((double) av.yRotationRate);
data.angVels.get(2).add((double) av.zRotationRate);
data.angVelTimes.add(t.addSplit());
for (int i = 0; i < 3; i ++) {
data.angVels.get(i).times.add(t.addSplit());
}
data.angVels.get(0).values.add((double) av.xRotationRate);
data.angVels.get(1).values.add((double) av.yRotationRate);
data.angVels.get(2).values.add((double) av.zRotationRate);
}
for (DcMotorEx m : view.motors) {

36
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveView.java
View File

@ -3,11 +3,12 @@ package org.firstinspires.ftc.teamcode.tuning;
import com.acmerobotics.roadrunner.MotorFeedforward;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.Twist2d;
import com.qualcomm.robotcore.hardware.DcMotorController;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import com.qualcomm.robotcore.util.SerialNumber;
import org.firstinspires.ftc.teamcode.MecanumDrive;
import org.firstinspires.ftc.teamcode.TankDrive;
@ -15,13 +16,16 @@ import org.firstinspires.ftc.teamcode.ThreeDeadWheelLocalizer;
import org.firstinspires.ftc.teamcode.TwoDeadWheelLocalizer;
import org.firstinspires.ftc.teamcode.util.Encoder;
import org.firstinspires.ftc.teamcode.util.Localizer;
import org.firstinspires.ftc.teamcode.util.MidpointTimer;
import org.firstinspires.ftc.teamcode.util.OverflowEncoder;
import org.firstinspires.ftc.teamcode.util.RawEncoder;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
final class DriveView {
public final String type;
@ -29,6 +33,8 @@ final class DriveView {
public final double inPerTick;
public final double maxVel, minAccel, maxAccel;
public final List<LynxModule> lynxModules;
public final List<DcMotorEx> leftMotors, rightMotors;
public final List<DcMotorEx> motors;
@ -55,6 +61,8 @@ final class DriveView {
}
public DriveView(HardwareMap hardwareMap) {
lynxModules = hardwareMap.getAll(LynxModule.class);
final Localizer localizer;
if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
type = "mecanum";
@ -154,14 +162,6 @@ final class DriveView {
List<RawEncoder> allEncs = new ArrayList<>();
allEncs.addAll(forwardEncs);
allEncs.addAll(perpEncs);
DcMotorController c1 = allEncs.get(0).getController();
for (Encoder e : allEncs) {
DcMotorController c2 = e.getController();
if (c1 != c2) {
throw new IllegalArgumentException("all encoders must be attached to the same hub");
}
}
}
public MotorFeedforward feedforward() {
@ -189,4 +189,22 @@ final class DriveView {
throw new AssertionError();
}
public void setBulkCachingMode(LynxModule.BulkCachingMode mode) {
for (LynxModule m : lynxModules) {
m.setBulkCachingMode(mode);
}
}
public Map<SerialNumber, Double> resetAndBulkRead(MidpointTimer t) {
final Map<SerialNumber, Double> times = new HashMap<>();
for (LynxModule m : lynxModules) {
m.clearBulkCache();
t.addSplit();
m.getBulkData();
times.put(m.getSerialNumber(), t.addSplit());
}
return times;
}
}

60
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ForwardRampLogger.java
View File

@ -1,45 +1,62 @@
package org.firstinspires.ftc.teamcode.tuning;
import com.qualcomm.hardware.lynx.LynxDcMotorController;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.util.SerialNumber;
import org.firstinspires.ftc.teamcode.util.Encoder;
import org.firstinspires.ftc.teamcode.util.MidpointTimer;
import java.util.ArrayList;
import java.util.List;
import java.util.Map;
public final class ForwardRampLogger extends LinearOpMode {
private static double power(double seconds) {
return Math.min(0.1 * seconds, 0.9);
}
private static class Signal {
public final List<Double> times = new ArrayList<>();
public final List<Double> values = new ArrayList<>();
}
private static void recordEncoderData(Encoder e, Map<SerialNumber, Double> ts, Signal ps, Signal vs) {
SerialNumber sn = ((LynxDcMotorController) e.getController()).getSerialNumber();
Encoder.PositionVelocityPair p = e.getPositionAndVelocity();
ps.times.add(ts.get(sn));
ps.values.add((double) p.position);
vs.times.add(ts.get(sn));
vs.values.add((double) p.velocity);
}
@Override
public void runOpMode() throws InterruptedException {
DriveView view = new DriveView(hardwareMap);
view.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
class Data {
public final String type = view.type;
public final List<List<Double>> powerTimes = new ArrayList<>();
public final List<List<Double>> powers = new ArrayList<>();
public final List<Signal> powers = new ArrayList<>();
public final List<Double> voltageTimes = new ArrayList<>();
public final List<Double> voltages = new ArrayList<>();
public final Signal voltages = new Signal();
public final List<Double> encTimes = new ArrayList<>();
public final List<List<Integer>> forwardEncPositions = new ArrayList<>();
public final List<List<Integer>> forwardEncVels = new ArrayList<>();
public final List<Signal> forwardEncPositions = new ArrayList<>();
public final List<Signal> forwardEncVels = new ArrayList<>();
}
Data data = new Data();
for (DcMotorEx m : view.motors) {
data.powerTimes.add(new ArrayList<>());
data.powers.add(new ArrayList<>());
data.powers.add(new Signal());
}
for (Encoder e : view.forwardEncs) {
data.forwardEncPositions.add(new ArrayList<>());
data.forwardEncVels.add(new ArrayList<>());
data.forwardEncPositions.add(new Signal());
data.forwardEncVels.add(new Signal());
}
waitForStart();
@ -50,19 +67,24 @@ public final class ForwardRampLogger extends LinearOpMode {
double power = power(t.seconds());
view.motors.get(i).setPower(power);
data.powers.get(i).add(power);
data.powerTimes.get(i).add(t.addSplit());
Signal s = data.powers.get(i);
s.times.add(t.addSplit());
s.values.add(power);
}
data.voltages.add(view.voltageSensor.getVoltage());
data.voltageTimes.add(t.addSplit());
data.voltages.values.add(view.voltageSensor.getVoltage());
data.voltages.times.add(t.addSplit());
Map<SerialNumber, Double> encTimes = view.resetAndBulkRead(t);
for (int i = 0; i < view.forwardEncs.size(); i++) {
Encoder.PositionVelocityPair p = view.forwardEncs.get(i).getPositionAndVelocity();
data.forwardEncPositions.get(i).add(p.position);
data.forwardEncVels.get(i).add(p.velocity);
recordEncoderData(
view.forwardEncs.get(i),
encTimes,
data.forwardEncPositions.get(i),
data.forwardEncVels.get(i)
);
}
data.encTimes.add(t.addSplit());
}
for (DcMotorEx m : view.motors) {