From 332704b5363d21e0c77ae60090f6eba553003a68 Mon Sep 17 00:00:00 2001 From: Ryan Brott Date: Sun, 5 Mar 2023 21:01:03 -0800 Subject: [PATCH] Record signals on Java side --- .../teamcode/tuning/AngularRampLogger.java | 137 +++++++++++------- .../ftc/teamcode/tuning/DriveView.java | 36 +++-- .../teamcode/tuning/ForwardRampLogger.java | 60 +++++--- 3 files changed, 152 insertions(+), 81 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/AngularRampLogger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/AngularRampLogger.java index 5fb6f07..8a404ef 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/AngularRampLogger.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/AngularRampLogger.java @@ -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 times = new ArrayList<>(); + public final List values = new ArrayList<>(); + } + + private static void recordEncoderData(Encoder e, Map 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> leftPowerTimes = new ArrayList<>(); - public final List> leftPowers = new ArrayList<>(); + public final List leftPowers = new ArrayList<>(); + public final List rightPowers = new ArrayList<>(); - public final List> rightPowerTimes = new ArrayList<>(); - public final List> rightPowers = new ArrayList<>(); + public final Signal voltages = new Signal(); - public final List voltageTimes = new ArrayList<>(); - public final List voltages = new ArrayList<>(); + public final List leftEncPositions = new ArrayList<>(); + public final List rightEncPositions = new ArrayList<>(); + public final List parEncPositions = new ArrayList<>(); + public final List perpEncPositions = new ArrayList<>(); - public final List encTimes = new ArrayList<>(); - public final List> leftEncPositions = new ArrayList<>(); - public final List> leftEncVels = new ArrayList<>(); - public final List> rightEncPositions = new ArrayList<>(); - public final List> rightEncVels = new ArrayList<>(); - public final List> parEncPositions = new ArrayList<>(); - public final List> parEncVels = new ArrayList<>(); - public final List> perpEncPositions = new ArrayList<>(); - public final List> perpEncVels = new ArrayList<>(); + public final List leftEncVels = new ArrayList<>(); + public final List rightEncVels = new ArrayList<>(); + public final List parEncVels = new ArrayList<>(); + public final List perpEncVels = new ArrayList<>(); - public final List angVelTimes = new ArrayList<>(); - public final List> angVels = new ArrayList<>(); + public final List 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 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) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveView.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveView.java index 54fdfb1..a18ef76 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveView.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveView.java @@ -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 lynxModules; + public final List leftMotors, rightMotors; public final List 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 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 resetAndBulkRead(MidpointTimer t) { + final Map times = new HashMap<>(); + for (LynxModule m : lynxModules) { + m.clearBulkCache(); + + t.addSplit(); + m.getBulkData(); + times.put(m.getSerialNumber(), t.addSplit()); + } + return times; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ForwardRampLogger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ForwardRampLogger.java index 81f6afc..aed0588 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ForwardRampLogger.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ForwardRampLogger.java @@ -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 times = new ArrayList<>(); + public final List values = new ArrayList<>(); + } + + private static void recordEncoderData(Encoder e, Map 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> powerTimes = new ArrayList<>(); - public final List> powers = new ArrayList<>(); + public final List powers = new ArrayList<>(); - public final List voltageTimes = new ArrayList<>(); - public final List voltages = new ArrayList<>(); + public final Signal voltages = new Signal(); - public final List encTimes = new ArrayList<>(); - public final List> forwardEncPositions = new ArrayList<>(); - public final List> forwardEncVels = new ArrayList<>(); + public final List forwardEncPositions = new ArrayList<>(); + public final List 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 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) {