Skip to content

Commit

Permalink
Record signals on Java side
Browse files Browse the repository at this point in the history
  • Loading branch information
@rbrott
rbrott committed Mar 6, 2023
1 parent 47151cb commit 332704b
Show file tree
Hide file tree
Showing 3 changed files with 152 additions and 81 deletions.
137 changes: 84 additions & 53 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/AngularRampLogger.java
View file
Original file line number Diff line number Diff line change
@@ -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;
Expand All @@ -10,69 +13,80 @@

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();
Expand All @@ -83,48 +97,65 @@ class Data {
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) {
Expand Down
36 changes: 27 additions & 9 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveView.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,32 +3,38 @@
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;
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;

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;

Expand All @@ -55,6 +61,8 @@ private static RawEncoder unwrap(Encoder e) {
}

public DriveView(HardwareMap hardwareMap) {
lynxModules = hardwareMap.getAll(LynxModule.class);

final Localizer localizer;
if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
type = "mecanum";
Expand Down Expand Up @@ -154,14 +162,6 @@ public DriveView(HardwareMap hardwareMap) {
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() {
Expand Down Expand Up @@ -189,4 +189,22 @@ public void setDrivePowers(Twist2d powers) {

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;
}
}
Loading

0 comments on commit 332704b

Please sign in to comment.