From 7bd8c4457034f1899a7beda907bc7fde26f0393a Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Sat, 9 Mar 2024 12:09:02 -0600 Subject: [PATCH 01/12] [wpimath] Add structured data support for DifferentialDriveWheelPositions (#6412) --- .../edu/wpi/first/math/proto/Kinematics.java | 460 +++++++++++++++--- .../DifferentialDriveWheelPositions.java | 10 + .../DifferentialDriveWheelPositionsProto.java | 40 ++ ...DifferentialDriveWheelPositionsStruct.java | 45 ++ .../DifferentialDriveWheelPositionsProto.cpp | 34 ++ .../DifferentialDriveWheelPositionsStruct.cpp | 26 + .../DifferentialDriveWheelPositions.h | 3 + .../DifferentialDriveWheelPositionsProto.h | 19 + .../DifferentialDriveWheelPositionsStruct.h | 28 ++ wpimath/src/main/proto/kinematics.proto | 5 + ...erentialDriveWheelPositionsStructTest.java | 29 ++ ...ferentialDriveWheelPositionsStructTest.cpp | 27 + 12 files changed, 668 insertions(+), 58 deletions(-) create mode 100644 wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelPositionsProto.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStruct.java create mode 100644 wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelPositionsProto.cpp create mode 100644 wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelPositionsStruct.cpp create mode 100644 wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelPositionsProto.h create mode 100644 wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelPositionsStruct.h create mode 100644 wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStructTest.java create mode 100644 wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelPositionsStructTest.cpp diff --git a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java index bd0c4450cb..5494f0b675 100644 --- a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java +++ b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java @@ -19,61 +19,65 @@ import us.hebi.quickbuf.RepeatedByte; import us.hebi.quickbuf.RepeatedMessage; public final class Kinematics { - private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(3208, + private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(3427, "ChBraW5lbWF0aWNzLnByb3RvEgl3cGkucHJvdG8aEGdlb21ldHJ5MmQucHJvdG8iTQoVUHJvdG9idWZD" + "aGFzc2lzU3BlZWRzEg4KAnZ4GAEgASgBUgJ2eBIOCgJ2eRgCIAEoAVICdnkSFAoFb21lZ2EYAyABKAFS" + "BW9tZWdhIkYKI1Byb3RvYnVmRGlmZmVyZW50aWFsRHJpdmVLaW5lbWF0aWNzEh8KC3RyYWNrX3dpZHRo" + "GAEgASgBUgp0cmFja1dpZHRoIlAKJFByb3RvYnVmRGlmZmVyZW50aWFsRHJpdmVXaGVlbFNwZWVkcxIS" + - "CgRsZWZ0GAEgASgBUgRsZWZ0EhQKBXJpZ2h0GAIgASgBUgVyaWdodCKkAgoeUHJvdG9idWZNZWNhbnVt" + - "RHJpdmVLaW5lbWF0aWNzEj8KCmZyb250X2xlZnQYASABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFu" + - "c2xhdGlvbjJkUglmcm9udExlZnQSQQoLZnJvbnRfcmlnaHQYAiABKAsyIC53cGkucHJvdG8uUHJvdG9i" + - "dWZUcmFuc2xhdGlvbjJkUgpmcm9udFJpZ2h0Ej0KCXJlYXJfbGVmdBgDIAEoCzIgLndwaS5wcm90by5Q" + - "cm90b2J1ZlRyYW5zbGF0aW9uMmRSCHJlYXJMZWZ0Ej8KCnJlYXJfcmlnaHQYBCABKAsyIC53cGkucHJv" + - "dG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUglyZWFyUmlnaHQinwEKIVByb3RvYnVmTWVjYW51bURyaXZl" + - "TW90b3JWb2x0YWdlcxIdCgpmcm9udF9sZWZ0GAEgASgBUglmcm9udExlZnQSHwoLZnJvbnRfcmlnaHQY" + - "AiABKAFSCmZyb250UmlnaHQSGwoJcmVhcl9sZWZ0GAMgASgBUghyZWFyTGVmdBIdCgpyZWFyX3JpZ2h0" + - "GAQgASgBUglyZWFyUmlnaHQioAEKIlByb3RvYnVmTWVjYW51bURyaXZlV2hlZWxQb3NpdGlvbnMSHQoK" + - "ZnJvbnRfbGVmdBgBIAEoAVIJZnJvbnRMZWZ0Eh8KC2Zyb250X3JpZ2h0GAIgASgBUgpmcm9udFJpZ2h0" + - "EhsKCXJlYXJfbGVmdBgDIAEoAVIIcmVhckxlZnQSHQoKcmVhcl9yaWdodBgEIAEoAVIJcmVhclJpZ2h0" + - "Ip0BCh9Qcm90b2J1Zk1lY2FudW1Ecml2ZVdoZWVsU3BlZWRzEh0KCmZyb250X2xlZnQYASABKAFSCWZy" + - "b250TGVmdBIfCgtmcm9udF9yaWdodBgCIAEoAVIKZnJvbnRSaWdodBIbCglyZWFyX2xlZnQYAyABKAFS" + - "CHJlYXJMZWZ0Eh0KCnJlYXJfcmlnaHQYBCABKAFSCXJlYXJSaWdodCJbCh1Qcm90b2J1ZlN3ZXJ2ZURy" + - "aXZlS2luZW1hdGljcxI6Cgdtb2R1bGVzGAEgAygLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRp" + - "b24yZFIHbW9kdWxlcyJvChxQcm90b2J1ZlN3ZXJ2ZU1vZHVsZVBvc2l0aW9uEhoKCGRpc3RhbmNlGAEg", - "ASgBUghkaXN0YW5jZRIzCgVhbmdsZRgCIAEoCzIdLndwaS5wcm90by5Qcm90b2J1ZlJvdGF0aW9uMmRS" + - "BWFuZ2xlImYKGVByb3RvYnVmU3dlcnZlTW9kdWxlU3RhdGUSFAoFc3BlZWQYASABKAFSBXNwZWVkEjMK" + - "BWFuZ2xlGAIgASgLMh0ud3BpLnByb3RvLlByb3RvYnVmUm90YXRpb24yZFIFYW5nbGVCGgoYZWR1Lndw" + - "aS5maXJzdC5tYXRoLnByb3RvSocOCgYSBAAAPwEKCAoBDBIDAAASCggKAQISAwIAEgoJCgIDABIDBAAa" + - "CggKAQgSAwYAMQoJCgIIARIDBgAxCgoKAgQAEgQIAAwBCgoKAwQAARIDCAgdCgsKBAQAAgASAwkCEAoM" + - "CgUEAAIABRIDCQIICgwKBQQAAgABEgMJCQsKDAoFBAACAAMSAwkODwoLCgQEAAIBEgMKAhAKDAoFBAAC" + - "AQUSAwoCCAoMCgUEAAIBARIDCgkLCgwKBQQAAgEDEgMKDg8KCwoEBAACAhIDCwITCgwKBQQAAgIFEgML" + - "AggKDAoFBAACAgESAwsJDgoMCgUEAAICAxIDCxESCgoKAgQBEgQOABABCgoKAwQBARIDDggrCgsKBAQB" + - "AgASAw8CGQoMCgUEAQIABRIDDwIICgwKBQQBAgABEgMPCRQKDAoFBAECAAMSAw8XGAoKCgIEAhIEEgAV" + - "AQoKCgMEAgESAxIILAoLCgQEAgIAEgMTAhIKDAoFBAICAAUSAxMCCAoMCgUEAgIAARIDEwkNCgwKBQQC" + - "AgADEgMTEBEKCwoEBAICARIDFAITCgwKBQQCAgEFEgMUAggKDAoFBAICAQESAxQJDgoMCgUEAgIBAxID" + - "FBESCgoKAgQDEgQXABwBCgoKAwQDARIDFwgmCgsKBAQDAgASAxgCJwoMCgUEAwIABhIDGAIXCgwKBQQD" + - "AgABEgMYGCIKDAoFBAMCAAMSAxglJgoLCgQEAwIBEgMZAigKDAoFBAMCAQYSAxkCFwoMCgUEAwIBARID" + - "GRgjCgwKBQQDAgEDEgMZJicKCwoEBAMCAhIDGgImCgwKBQQDAgIGEgMaAhcKDAoFBAMCAgESAxoYIQoM" + - "CgUEAwICAxIDGiQlCgsKBAQDAgMSAxsCJwoMCgUEAwIDBhIDGwIXCgwKBQQDAgMBEgMbGCIKDAoFBAMC" + - "AwMSAxslJgoKCgIEBBIEHgAjAQoKCgMEBAESAx4IKQoLCgQEBAIAEgMfAhgKDAoFBAQCAAUSAx8CCAoM" + - "CgUEBAIAARIDHwkTCgwKBQQEAgADEgMfFhcKCwoEBAQCARIDIAIZCgwKBQQEAgEFEgMgAggKDAoFBAQC" + - "AQESAyAJFAoMCgUEBAIBAxIDIBcYCgsKBAQEAgISAyECFwoMCgUEBAICBRIDIQIICgwKBQQEAgIBEgMh" + - "CRIKDAoFBAQCAgMSAyEVFgoLCgQEBAIDEgMiAhgKDAoFBAQCAwUSAyICCAoMCgUEBAIDARIDIgkTCgwK" + - "BQQEAgMDEgMiFhcKCgoCBAUSBCUAKgEKCgoDBAUBEgMlCCoKCwoEBAUCABIDJgIYCgwKBQQFAgAFEgMm", - "AggKDAoFBAUCAAESAyYJEwoMCgUEBQIAAxIDJhYXCgsKBAQFAgESAycCGQoMCgUEBQIBBRIDJwIICgwK" + - "BQQFAgEBEgMnCRQKDAoFBAUCAQMSAycXGAoLCgQEBQICEgMoAhcKDAoFBAUCAgUSAygCCAoMCgUEBQIC" + - "ARIDKAkSCgwKBQQFAgIDEgMoFRYKCwoEBAUCAxIDKQIYCgwKBQQFAgMFEgMpAggKDAoFBAUCAwESAykJ" + - "EwoMCgUEBQIDAxIDKRYXCgoKAgQGEgQsADEBCgoKAwQGARIDLAgnCgsKBAQGAgASAy0CGAoMCgUEBgIA" + - "BRIDLQIICgwKBQQGAgABEgMtCRMKDAoFBAYCAAMSAy0WFwoLCgQEBgIBEgMuAhkKDAoFBAYCAQUSAy4C" + - "CAoMCgUEBgIBARIDLgkUCgwKBQQGAgEDEgMuFxgKCwoEBAYCAhIDLwIXCgwKBQQGAgIFEgMvAggKDAoF" + - "BAYCAgESAy8JEgoMCgUEBgICAxIDLxUWCgsKBAQGAgMSAzACGAoMCgUEBgIDBRIDMAIICgwKBQQGAgMB" + - "EgMwCRMKDAoFBAYCAwMSAzAWFwoKCgIEBxIEMwA1AQoKCgMEBwESAzMIJQoLCgQEBwIAEgM0Ai0KDAoF" + - "BAcCAAQSAzQCCgoMCgUEBwIABhIDNAsgCgwKBQQHAgABEgM0ISgKDAoFBAcCAAMSAzQrLAoKCgIECBIE" + - "NwA6AQoKCgMECAESAzcIJAoLCgQECAIAEgM4AhYKDAoFBAgCAAUSAzgCCAoMCgUECAIAARIDOAkRCgwK" + - "BQQIAgADEgM4FBUKCwoEBAgCARIDOQIfCgwKBQQIAgEGEgM5AhQKDAoFBAgCAQESAzkVGgoMCgUECAIB" + - "AxIDOR0eCgoKAgQJEgQ8AD8BCgoKAwQJARIDPAghCgsKBAQJAgASAz0CEwoMCgUECQIABRIDPQIICgwK" + - "BQQJAgABEgM9CQ4KDAoFBAkCAAMSAz0REgoLCgQECQIBEgM+Ah8KDAoFBAkCAQYSAz4CFAoMCgUECQIB" + - "ARIDPhUaCgwKBQQJAgEDEgM+HR5iBnByb3RvMw=="); + "CgRsZWZ0GAEgASgBUgRsZWZ0EhQKBXJpZ2h0GAIgASgBUgVyaWdodCJTCidQcm90b2J1ZkRpZmZlcmVu" + + "dGlhbERyaXZlV2hlZWxQb3NpdGlvbnMSEgoEbGVmdBgBIAEoAVIEbGVmdBIUCgVyaWdodBgCIAEoAVIF" + + "cmlnaHQipAIKHlByb3RvYnVmTWVjYW51bURyaXZlS2luZW1hdGljcxI/Cgpmcm9udF9sZWZ0GAEgASgL" + + "MiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIJZnJvbnRMZWZ0EkEKC2Zyb250X3JpZ2h0" + + "GAIgASgLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIKZnJvbnRSaWdodBI9CglyZWFy" + + "X2xlZnQYAyABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUghyZWFyTGVmdBI/Cgpy" + + "ZWFyX3JpZ2h0GAQgASgLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIJcmVhclJpZ2h0" + + "Ip8BCiFQcm90b2J1Zk1lY2FudW1Ecml2ZU1vdG9yVm9sdGFnZXMSHQoKZnJvbnRfbGVmdBgBIAEoAVIJ" + + "ZnJvbnRMZWZ0Eh8KC2Zyb250X3JpZ2h0GAIgASgBUgpmcm9udFJpZ2h0EhsKCXJlYXJfbGVmdBgDIAEo" + + "AVIIcmVhckxlZnQSHQoKcmVhcl9yaWdodBgEIAEoAVIJcmVhclJpZ2h0IqABCiJQcm90b2J1Zk1lY2Fu" + + "dW1Ecml2ZVdoZWVsUG9zaXRpb25zEh0KCmZyb250X2xlZnQYASABKAFSCWZyb250TGVmdBIfCgtmcm9u" + + "dF9yaWdodBgCIAEoAVIKZnJvbnRSaWdodBIbCglyZWFyX2xlZnQYAyABKAFSCHJlYXJMZWZ0Eh0KCnJl" + + "YXJfcmlnaHQYBCABKAFSCXJlYXJSaWdodCKdAQofUHJvdG9idWZNZWNhbnVtRHJpdmVXaGVlbFNwZWVk" + + "cxIdCgpmcm9udF9sZWZ0GAEgASgBUglmcm9udExlZnQSHwoLZnJvbnRfcmlnaHQYAiABKAFSCmZyb250" + + "UmlnaHQSGwoJcmVhcl9sZWZ0GAMgASgBUghyZWFyTGVmdBIdCgpyZWFyX3JpZ2h0GAQgASgBUglyZWFy" + + "UmlnaHQiWwodUHJvdG9idWZTd2VydmVEcml2ZUtpbmVtYXRpY3MSOgoHbW9kdWxlcxgBIAMoCzIgLndw", + "aS5wcm90by5Qcm90b2J1ZlRyYW5zbGF0aW9uMmRSB21vZHVsZXMibwocUHJvdG9idWZTd2VydmVNb2R1" + + "bGVQb3NpdGlvbhIaCghkaXN0YW5jZRgBIAEoAVIIZGlzdGFuY2USMwoFYW5nbGUYAiABKAsyHS53cGku" + + "cHJvdG8uUHJvdG9idWZSb3RhdGlvbjJkUgVhbmdsZSJmChlQcm90b2J1ZlN3ZXJ2ZU1vZHVsZVN0YXRl" + + "EhQKBXNwZWVkGAEgASgBUgVzcGVlZBIzCgVhbmdsZRgCIAEoCzIdLndwaS5wcm90by5Qcm90b2J1ZlJv" + + "dGF0aW9uMmRSBWFuZ2xlQhoKGGVkdS53cGkuZmlyc3QubWF0aC5wcm90b0qNDwoGEgQAAEQBCggKAQwS" + + "AwAAEgoICgECEgMCABIKCQoCAwASAwQAGgoICgEIEgMGADEKCQoCCAESAwYAMQoKCgIEABIECAAMAQoK" + + "CgMEAAESAwgIHQoLCgQEAAIAEgMJAhAKDAoFBAACAAUSAwkCCAoMCgUEAAIAARIDCQkLCgwKBQQAAgAD" + + "EgMJDg8KCwoEBAACARIDCgIQCgwKBQQAAgEFEgMKAggKDAoFBAACAQESAwoJCwoMCgUEAAIBAxIDCg4P" + + "CgsKBAQAAgISAwsCEwoMCgUEAAICBRIDCwIICgwKBQQAAgIBEgMLCQ4KDAoFBAACAgMSAwsREgoKCgIE" + + "ARIEDgAQAQoKCgMEAQESAw4IKwoLCgQEAQIAEgMPAhkKDAoFBAECAAUSAw8CCAoMCgUEAQIAARIDDwkU" + + "CgwKBQQBAgADEgMPFxgKCgoCBAISBBIAFQEKCgoDBAIBEgMSCCwKCwoEBAICABIDEwISCgwKBQQCAgAF" + + "EgMTAggKDAoFBAICAAESAxMJDQoMCgUEAgIAAxIDExARCgsKBAQCAgESAxQCEwoMCgUEAgIBBRIDFAII" + + "CgwKBQQCAgEBEgMUCQ4KDAoFBAICAQMSAxQREgoKCgIEAxIEFwAaAQoKCgMEAwESAxcILwoLCgQEAwIA" + + "EgMYAhIKDAoFBAMCAAUSAxgCCAoMCgUEAwIAARIDGAkNCgwKBQQDAgADEgMYEBEKCwoEBAMCARIDGQIT" + + "CgwKBQQDAgEFEgMZAggKDAoFBAMCAQESAxkJDgoMCgUEAwIBAxIDGRESCgoKAgQEEgQcACEBCgoKAwQE" + + "ARIDHAgmCgsKBAQEAgASAx0CJwoMCgUEBAIABhIDHQIXCgwKBQQEAgABEgMdGCIKDAoFBAQCAAMSAx0l" + + "JgoLCgQEBAIBEgMeAigKDAoFBAQCAQYSAx4CFwoMCgUEBAIBARIDHhgjCgwKBQQEAgEDEgMeJicKCwoE" + + "BAQCAhIDHwImCgwKBQQEAgIGEgMfAhcKDAoFBAQCAgESAx8YIQoMCgUEBAICAxIDHyQlCgsKBAQEAgMS" + + "AyACJwoMCgUEBAIDBhIDIAIXCgwKBQQEAgMBEgMgGCIKDAoFBAQCAwMSAyAlJgoKCgIEBRIEIwAoAQoK" + + "CgMEBQESAyMIKQoLCgQEBQIAEgMkAhgKDAoFBAUCAAUSAyQCCAoMCgUEBQIAARIDJAkTCgwKBQQFAgAD", + "EgMkFhcKCwoEBAUCARIDJQIZCgwKBQQFAgEFEgMlAggKDAoFBAUCAQESAyUJFAoMCgUEBQIBAxIDJRcY" + + "CgsKBAQFAgISAyYCFwoMCgUEBQICBRIDJgIICgwKBQQFAgIBEgMmCRIKDAoFBAUCAgMSAyYVFgoLCgQE" + + "BQIDEgMnAhgKDAoFBAUCAwUSAycCCAoMCgUEBQIDARIDJwkTCgwKBQQFAgMDEgMnFhcKCgoCBAYSBCoA" + + "LwEKCgoDBAYBEgMqCCoKCwoEBAYCABIDKwIYCgwKBQQGAgAFEgMrAggKDAoFBAYCAAESAysJEwoMCgUE" + + "BgIAAxIDKxYXCgsKBAQGAgESAywCGQoMCgUEBgIBBRIDLAIICgwKBQQGAgEBEgMsCRQKDAoFBAYCAQMS" + + "AywXGAoLCgQEBgICEgMtAhcKDAoFBAYCAgUSAy0CCAoMCgUEBgICARIDLQkSCgwKBQQGAgIDEgMtFRYK" + + "CwoEBAYCAxIDLgIYCgwKBQQGAgMFEgMuAggKDAoFBAYCAwESAy4JEwoMCgUEBgIDAxIDLhYXCgoKAgQH" + + "EgQxADYBCgoKAwQHARIDMQgnCgsKBAQHAgASAzICGAoMCgUEBwIABRIDMgIICgwKBQQHAgABEgMyCRMK" + + "DAoFBAcCAAMSAzIWFwoLCgQEBwIBEgMzAhkKDAoFBAcCAQUSAzMCCAoMCgUEBwIBARIDMwkUCgwKBQQH" + + "AgEDEgMzFxgKCwoEBAcCAhIDNAIXCgwKBQQHAgIFEgM0AggKDAoFBAcCAgESAzQJEgoMCgUEBwICAxID" + + "NBUWCgsKBAQHAgMSAzUCGAoMCgUEBwIDBRIDNQIICgwKBQQHAgMBEgM1CRMKDAoFBAcCAwMSAzUWFwoK" + + "CgIECBIEOAA6AQoKCgMECAESAzgIJQoLCgQECAIAEgM5Ai0KDAoFBAgCAAQSAzkCCgoMCgUECAIABhID" + + "OQsgCgwKBQQIAgABEgM5ISgKDAoFBAgCAAMSAzkrLAoKCgIECRIEPAA/AQoKCgMECQESAzwIJAoLCgQE" + + "CQIAEgM9AhYKDAoFBAkCAAUSAz0CCAoMCgUECQIAARIDPQkRCgwKBQQJAgADEgM9FBUKCwoEBAkCARID" + + "PgIfCgwKBQQJAgEGEgM+AhQKDAoFBAkCAQESAz4VGgoMCgUECQIBAxIDPh0eCgoKAgQKEgRBAEQBCgoK" + + "AwQKARIDQQghCgsKBAQKAgASA0ICEwoMCgUECgIABRIDQgIICgwKBQQKAgABEgNCCQ4KDAoFBAoCAAMS" + + "A0IREgoLCgQECgIBEgNDAh8KDAoFBAoCAQYSA0MCFAoMCgUECgIBARIDQxUaCgwKBQQKAgEDEgNDHR5i" + + "BnByb3RvMw=="); static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("kinematics.proto", "wpi.proto", descriptorData, Geometry2D.getDescriptor()); @@ -83,19 +87,21 @@ public final class Kinematics { static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelSpeeds_descriptor = descriptor.internalContainedType(200, 80, "ProtobufDifferentialDriveWheelSpeeds", "wpi.proto.ProtobufDifferentialDriveWheelSpeeds"); - static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveKinematics_descriptor = descriptor.internalContainedType(283, 292, "ProtobufMecanumDriveKinematics", "wpi.proto.ProtobufMecanumDriveKinematics"); + static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelPositions_descriptor = descriptor.internalContainedType(282, 83, "ProtobufDifferentialDriveWheelPositions", "wpi.proto.ProtobufDifferentialDriveWheelPositions"); - static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveMotorVoltages_descriptor = descriptor.internalContainedType(578, 159, "ProtobufMecanumDriveMotorVoltages", "wpi.proto.ProtobufMecanumDriveMotorVoltages"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveKinematics_descriptor = descriptor.internalContainedType(368, 292, "ProtobufMecanumDriveKinematics", "wpi.proto.ProtobufMecanumDriveKinematics"); - static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelPositions_descriptor = descriptor.internalContainedType(740, 160, "ProtobufMecanumDriveWheelPositions", "wpi.proto.ProtobufMecanumDriveWheelPositions"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveMotorVoltages_descriptor = descriptor.internalContainedType(663, 159, "ProtobufMecanumDriveMotorVoltages", "wpi.proto.ProtobufMecanumDriveMotorVoltages"); - static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelSpeeds_descriptor = descriptor.internalContainedType(903, 157, "ProtobufMecanumDriveWheelSpeeds", "wpi.proto.ProtobufMecanumDriveWheelSpeeds"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelPositions_descriptor = descriptor.internalContainedType(825, 160, "ProtobufMecanumDriveWheelPositions", "wpi.proto.ProtobufMecanumDriveWheelPositions"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveDriveKinematics_descriptor = descriptor.internalContainedType(1062, 91, "ProtobufSwerveDriveKinematics", "wpi.proto.ProtobufSwerveDriveKinematics"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelSpeeds_descriptor = descriptor.internalContainedType(988, 157, "ProtobufMecanumDriveWheelSpeeds", "wpi.proto.ProtobufMecanumDriveWheelSpeeds"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModulePosition_descriptor = descriptor.internalContainedType(1155, 111, "ProtobufSwerveModulePosition", "wpi.proto.ProtobufSwerveModulePosition"); + static final Descriptors.Descriptor wpi_proto_ProtobufSwerveDriveKinematics_descriptor = descriptor.internalContainedType(1147, 91, "ProtobufSwerveDriveKinematics", "wpi.proto.ProtobufSwerveDriveKinematics"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleState_descriptor = descriptor.internalContainedType(1268, 102, "ProtobufSwerveModuleState", "wpi.proto.ProtobufSwerveModuleState"); + static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModulePosition_descriptor = descriptor.internalContainedType(1240, 111, "ProtobufSwerveModulePosition", "wpi.proto.ProtobufSwerveModulePosition"); + + static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleState_descriptor = descriptor.internalContainedType(1353, 102, "ProtobufSwerveModuleState", "wpi.proto.ProtobufSwerveModuleState"); /** * @return this proto file's descriptor. @@ -1113,6 +1119,344 @@ public final class Kinematics { } } + /** + * Protobuf type {@code ProtobufDifferentialDriveWheelPositions} + */ + public static final class ProtobufDifferentialDriveWheelPositions extends ProtoMessage implements Cloneable { + private static final long serialVersionUID = 0L; + + /** + * optional double left = 1; + */ + private double left; + + /** + * optional double right = 2; + */ + private double right; + + private ProtobufDifferentialDriveWheelPositions() { + } + + /** + * @return a new empty instance of {@code ProtobufDifferentialDriveWheelPositions} + */ + public static ProtobufDifferentialDriveWheelPositions newInstance() { + return new ProtobufDifferentialDriveWheelPositions(); + } + + /** + * optional double left = 1; + * @return whether the left field is set + */ + public boolean hasLeft() { + return (bitField0_ & 0x00000001) != 0; + } + + /** + * optional double left = 1; + * @return this + */ + public ProtobufDifferentialDriveWheelPositions clearLeft() { + bitField0_ &= ~0x00000001; + left = 0D; + return this; + } + + /** + * optional double left = 1; + * @return the left + */ + public double getLeft() { + return left; + } + + /** + * optional double left = 1; + * @param value the left to set + * @return this + */ + public ProtobufDifferentialDriveWheelPositions setLeft(final double value) { + bitField0_ |= 0x00000001; + left = value; + return this; + } + + /** + * optional double right = 2; + * @return whether the right field is set + */ + public boolean hasRight() { + return (bitField0_ & 0x00000002) != 0; + } + + /** + * optional double right = 2; + * @return this + */ + public ProtobufDifferentialDriveWheelPositions clearRight() { + bitField0_ &= ~0x00000002; + right = 0D; + return this; + } + + /** + * optional double right = 2; + * @return the right + */ + public double getRight() { + return right; + } + + /** + * optional double right = 2; + * @param value the right to set + * @return this + */ + public ProtobufDifferentialDriveWheelPositions setRight(final double value) { + bitField0_ |= 0x00000002; + right = value; + return this; + } + + @Override + public ProtobufDifferentialDriveWheelPositions copyFrom( + final ProtobufDifferentialDriveWheelPositions other) { + cachedSize = other.cachedSize; + if ((bitField0_ | other.bitField0_) != 0) { + bitField0_ = other.bitField0_; + left = other.left; + right = other.right; + } + return this; + } + + @Override + public ProtobufDifferentialDriveWheelPositions mergeFrom( + final ProtobufDifferentialDriveWheelPositions other) { + if (other.isEmpty()) { + return this; + } + cachedSize = -1; + if (other.hasLeft()) { + setLeft(other.left); + } + if (other.hasRight()) { + setRight(other.right); + } + return this; + } + + @Override + public ProtobufDifferentialDriveWheelPositions clear() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + left = 0D; + right = 0D; + return this; + } + + @Override + public ProtobufDifferentialDriveWheelPositions clearQuick() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + return this; + } + + @Override + public boolean equals(Object o) { + if (o == this) { + return true; + } + if (!(o instanceof ProtobufDifferentialDriveWheelPositions)) { + return false; + } + ProtobufDifferentialDriveWheelPositions other = (ProtobufDifferentialDriveWheelPositions) o; + return bitField0_ == other.bitField0_ + && (!hasLeft() || ProtoUtil.isEqual(left, other.left)) + && (!hasRight() || ProtoUtil.isEqual(right, other.right)); + } + + @Override + public void writeTo(final ProtoSink output) throws IOException { + if ((bitField0_ & 0x00000001) != 0) { + output.writeRawByte((byte) 9); + output.writeDoubleNoTag(left); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeRawByte((byte) 17); + output.writeDoubleNoTag(right); + } + } + + @Override + protected int computeSerializedSize() { + int size = 0; + if ((bitField0_ & 0x00000001) != 0) { + size += 9; + } + if ((bitField0_ & 0x00000002) != 0) { + size += 9; + } + return size; + } + + @Override + @SuppressWarnings("fallthrough") + public ProtobufDifferentialDriveWheelPositions mergeFrom(final ProtoSource input) throws + IOException { + // Enabled Fall-Through Optimization (QuickBuffers) + int tag = input.readTag(); + while (true) { + switch (tag) { + case 9: { + // left + left = input.readDouble(); + bitField0_ |= 0x00000001; + tag = input.readTag(); + if (tag != 17) { + break; + } + } + case 17: { + // right + right = input.readDouble(); + bitField0_ |= 0x00000002; + tag = input.readTag(); + if (tag != 0) { + break; + } + } + case 0: { + return this; + } + default: { + if (!input.skipField(tag)) { + return this; + } + tag = input.readTag(); + break; + } + } + } + } + + @Override + public void writeTo(final JsonSink output) throws IOException { + output.beginObject(); + if ((bitField0_ & 0x00000001) != 0) { + output.writeDouble(FieldNames.left, left); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeDouble(FieldNames.right, right); + } + output.endObject(); + } + + @Override + public ProtobufDifferentialDriveWheelPositions mergeFrom(final JsonSource input) throws + IOException { + if (!input.beginObject()) { + return this; + } + while (!input.isAtEnd()) { + switch (input.readFieldHash()) { + case 3317767: { + if (input.isAtField(FieldNames.left)) { + if (!input.trySkipNullValue()) { + left = input.readDouble(); + bitField0_ |= 0x00000001; + } + } else { + input.skipUnknownField(); + } + break; + } + case 108511772: { + if (input.isAtField(FieldNames.right)) { + if (!input.trySkipNullValue()) { + right = input.readDouble(); + bitField0_ |= 0x00000002; + } + } else { + input.skipUnknownField(); + } + break; + } + default: { + input.skipUnknownField(); + break; + } + } + } + input.endObject(); + return this; + } + + @Override + public ProtobufDifferentialDriveWheelPositions clone() { + return new ProtobufDifferentialDriveWheelPositions().copyFrom(this); + } + + @Override + public boolean isEmpty() { + return ((bitField0_) == 0); + } + + public static ProtobufDifferentialDriveWheelPositions parseFrom(final byte[] data) throws + InvalidProtocolBufferException { + return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelPositions(), data).checkInitialized(); + } + + public static ProtobufDifferentialDriveWheelPositions parseFrom(final ProtoSource input) throws + IOException { + return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelPositions(), input).checkInitialized(); + } + + public static ProtobufDifferentialDriveWheelPositions parseFrom(final JsonSource input) throws + IOException { + return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelPositions(), input).checkInitialized(); + } + + /** + * @return factory for creating ProtobufDifferentialDriveWheelPositions messages + */ + public static MessageFactory getFactory() { + return ProtobufDifferentialDriveWheelPositionsFactory.INSTANCE; + } + + /** + * @return this type's descriptor. + */ + public static Descriptors.Descriptor getDescriptor() { + return Kinematics.wpi_proto_ProtobufDifferentialDriveWheelPositions_descriptor; + } + + private enum ProtobufDifferentialDriveWheelPositionsFactory implements MessageFactory { + INSTANCE; + + @Override + public ProtobufDifferentialDriveWheelPositions create() { + return ProtobufDifferentialDriveWheelPositions.newInstance(); + } + } + + /** + * Contains name constants used for serializing JSON + */ + static class FieldNames { + static final FieldName left = FieldName.forField("left"); + + static final FieldName right = FieldName.forField("right"); + } + } + /** * Protobuf type {@code ProtobufMecanumDriveKinematics} */ diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java index fbdf54d881..d6036b8747 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java @@ -7,6 +7,8 @@ package edu.wpi.first.math.kinematics; import static edu.wpi.first.units.Units.Meters; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.proto.DifferentialDriveWheelPositionsProto; +import edu.wpi.first.math.kinematics.struct.DifferentialDriveWheelPositionsStruct; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; import java.util.Objects; @@ -20,6 +22,14 @@ public class DifferentialDriveWheelPositions /** Distance measured by the right side. */ public double rightMeters; + /** DifferentialDriveWheelPostions struct for serialization. */ + public static final DifferentialDriveWheelPositionsStruct struct = + new DifferentialDriveWheelPositionsStruct(); + + /** DifferentialDriveWheelPostions struct for serialization. */ + public static final DifferentialDriveWheelPositionsProto proto = + new DifferentialDriveWheelPositionsProto(); + /** * Constructs a DifferentialDriveWheelPositions. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelPositionsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelPositionsProto.java new file mode 100644 index 0000000000..982756b5b7 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelPositionsProto.java @@ -0,0 +1,40 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.kinematics.proto; + +import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions; +import edu.wpi.first.math.proto.Kinematics.ProtobufDifferentialDriveWheelPositions; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class DifferentialDriveWheelPositionsProto + implements Protobuf { + @Override + public Class getTypeClass() { + return DifferentialDriveWheelPositions.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufDifferentialDriveWheelPositions.getDescriptor(); + } + + @Override + public ProtobufDifferentialDriveWheelPositions createMessage() { + return ProtobufDifferentialDriveWheelPositions.newInstance(); + } + + @Override + public DifferentialDriveWheelPositions unpack(ProtobufDifferentialDriveWheelPositions msg) { + return new DifferentialDriveWheelPositions(msg.getLeft(), msg.getRight()); + } + + @Override + public void pack( + ProtobufDifferentialDriveWheelPositions msg, DifferentialDriveWheelPositions value) { + msg.setLeft(value.leftMeters); + msg.setRight(value.rightMeters); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStruct.java new file mode 100644 index 0000000000..efa80ebd17 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStruct.java @@ -0,0 +1,45 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.kinematics.struct; + +import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class DifferentialDriveWheelPositionsStruct + implements Struct { + @Override + public Class getTypeClass() { + return DifferentialDriveWheelPositions.class; + } + + @Override + public String getTypeString() { + return "struct:DifferentialDriveWheelPositions"; + } + + @Override + public int getSize() { + return kSizeDouble * 2; + } + + @Override + public String getSchema() { + return "double left;double right"; + } + + @Override + public DifferentialDriveWheelPositions unpack(ByteBuffer bb) { + double left = bb.getDouble(); + double right = bb.getDouble(); + return new DifferentialDriveWheelPositions(left, right); + } + + @Override + public void pack(ByteBuffer bb, DifferentialDriveWheelPositions value) { + bb.putDouble(value.leftMeters); + bb.putDouble(value.rightMeters); + } +} diff --git a/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelPositionsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelPositionsProto.cpp new file mode 100644 index 0000000000..8a0af6f6fb --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelPositionsProto.cpp @@ -0,0 +1,34 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/kinematics/proto/DifferentialDriveWheelPositionsProto.h" + +#include "kinematics.pb.h" + +google::protobuf::Message* wpi::Protobuf< + frc::DifferentialDriveWheelPositions>::New(google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufDifferentialDriveWheelPositions>(arena); +} + +frc::DifferentialDriveWheelPositions +wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = + static_cast( + &msg); + return frc::DifferentialDriveWheelPositions{ + units::meter_t{m->left()}, + units::meter_t{m->right()}, + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, + const frc::DifferentialDriveWheelPositions& value) { + auto m = + static_cast(msg); + m->set_left(value.left.value()); + m->set_right(value.right.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelPositionsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelPositionsStruct.cpp new file mode 100644 index 0000000000..757ce62760 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelPositionsStruct.cpp @@ -0,0 +1,26 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/kinematics/struct/DifferentialDriveWheelPositionsStruct.h" + +namespace { +constexpr size_t kLeftOff = 0; +constexpr size_t kRightOff = kLeftOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::DifferentialDriveWheelPositions StructType::Unpack( + std::span data) { + return frc::DifferentialDriveWheelPositions{ + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::DifferentialDriveWheelPositions& value) { + wpi::PackStruct(data, value.left.value()); + wpi::PackStruct(data, value.right.value()); +} diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h index 4dddbeaf40..f22d6874f6 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h @@ -49,3 +49,6 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelPositions { } }; } // namespace frc + +#include "frc/kinematics/proto/DifferentialDriveWheelPositionsProto.h" +#include "frc/kinematics/struct/DifferentialDriveWheelPositionsStruct.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelPositionsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelPositionsProto.h new file mode 100644 index 0000000000..3cf44eaa40 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelPositionsProto.h @@ -0,0 +1,19 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "frc/kinematics/DifferentialDriveWheelPositions.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::DifferentialDriveWheelPositions Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::DifferentialDriveWheelPositions& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelPositionsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelPositionsStruct.h new file mode 100644 index 0000000000..ef694bc37b --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelPositionsStruct.h @@ -0,0 +1,28 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "frc/kinematics/DifferentialDriveWheelPositions.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view GetTypeString() { + return "struct:DifferentialDriveWheelPositions"; + } + static constexpr size_t GetSize() { return 16; } + static constexpr std::string_view GetSchema() { + return "double left;double right"; + } + + static frc::DifferentialDriveWheelPositions Unpack( + std::span data); + static void Pack(std::span data, + const frc::DifferentialDriveWheelPositions& value); +}; + +static_assert(wpi::StructSerializable); diff --git a/wpimath/src/main/proto/kinematics.proto b/wpimath/src/main/proto/kinematics.proto index 8fdbf2d8b0..4cb3b055a2 100644 --- a/wpimath/src/main/proto/kinematics.proto +++ b/wpimath/src/main/proto/kinematics.proto @@ -21,6 +21,11 @@ message ProtobufDifferentialDriveWheelSpeeds { double right = 2; } +message ProtobufDifferentialDriveWheelPositions { + double left = 1; + double right = 2; +} + message ProtobufMecanumDriveKinematics { ProtobufTranslation2d front_left = 1; ProtobufTranslation2d front_right = 2; diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStructTest.java new file mode 100644 index 0000000000..84cc9967e1 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStructTest.java @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.kinematics.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class DifferentialDriveWheelPositionsStructTest { + private static final DifferentialDriveWheelPositions DATA = + new DifferentialDriveWheelPositions(1.74, 35.04); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(DifferentialDriveWheelPositions.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + DifferentialDriveWheelPositions.struct.pack(buffer, DATA); + buffer.rewind(); + + DifferentialDriveWheelPositions data = DifferentialDriveWheelPositions.struct.unpack(buffer); + assertEquals(DATA.leftMeters, data.leftMeters); + assertEquals(DATA.rightMeters, data.rightMeters); + } +} diff --git a/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelPositionsStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelPositionsStructTest.cpp new file mode 100644 index 0000000000..59b52c25fb --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelPositionsStructTest.cpp @@ -0,0 +1,27 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include "frc/kinematics/DifferentialDriveWheelPositions.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const DifferentialDriveWheelPositions kExpectedData{ + DifferentialDriveWheelPositions{1.74_m, 35.04_m}}; +} // namespace + +TEST(DifferentialDriveWheelPositionsStructTest, Roundtrip) { + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); + StructType::Pack(buffer, kExpectedData); + + DifferentialDriveWheelPositions unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.left.value(), unpacked_data.left.value()); + EXPECT_EQ(kExpectedData.right.value(), unpacked_data.right.value()); +} From 973bb55e668314415c974e5248a82a2a92d70b51 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 10 Mar 2024 08:11:18 -0700 Subject: [PATCH 02/12] [wpimath] LinearSystemId: Don't throw if Kv = 0 (#6424) That's just a system with no back-EMF. --- .../math/system/plant/LinearSystemId.java | 18 +++++++++--------- .../include/frc/system/plant/LinearSystemId.h | 18 +++++++++--------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java index 2e7d4b94f2..bff21492ef 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java @@ -135,14 +135,14 @@ public final class LinearSystemId { *

u = K_v v + K_a a * * @param kV The velocity gain, in volts/(unit/sec) - * @param kA The acceleration gain, in volts/(unit/sec^2) + * @param kA The acceleration gain, in volts/(unit/sec²) * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if kV <= 0 or kA <= 0. + * @throws IllegalArgumentException if kV < 0 or kA <= 0. * @see https://github.com/wpilibsuite/sysid */ public static LinearSystem createDCMotorSystem(double kV, double kA) { - if (kV <= 0.0) { - throw new IllegalArgumentException("Kv must be greater than zero."); + if (kV < 0.0) { + throw new IllegalArgumentException("Kv must be greater than or equal to zero."); } if (kA <= 0.0) { throw new IllegalArgumentException("Ka must be greater than zero."); @@ -256,14 +256,14 @@ public final class LinearSystemId { *

u = K_v v + K_a a * * @param kV The velocity gain, in volts/(unit/sec) - * @param kA The acceleration gain, in volts/(unit/sec^2) + * @param kA The acceleration gain, in volts/(unit/sec²) * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if kV <= 0 or kA <= 0. + * @throws IllegalArgumentException if kV < 0 or kA <= 0. * @see https://github.com/wpilibsuite/sysid */ public static LinearSystem identifyVelocitySystem(double kV, double kA) { if (kV <= 0.0) { - throw new IllegalArgumentException("Kv must be greater than zero."); + throw new IllegalArgumentException("Kv must be greater than or equal to zero."); } if (kA <= 0.0) { throw new IllegalArgumentException("Ka must be greater than zero."); @@ -291,12 +291,12 @@ public final class LinearSystemId { * @param kV The velocity gain, in volts/(unit/sec) * @param kA The acceleration gain, in volts/(unit/sec²) * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if kV <= 0 or kA <= 0. + * @throws IllegalArgumentException if kV < 0 or kA <= 0. * @see https://github.com/wpilibsuite/sysid */ public static LinearSystem identifyPositionSystem(double kV, double kA) { if (kV <= 0.0) { - throw new IllegalArgumentException("Kv must be greater than zero."); + throw new IllegalArgumentException("Kv must be greater than or equal to zero."); } if (kA <= 0.0) { throw new IllegalArgumentException("Ka must be greater than zero."); diff --git a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h index ad5d3b0e28..2b1508c063 100644 --- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h +++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h @@ -79,7 +79,7 @@ class WPILIB_DLLEXPORT LinearSystemId { * * @param kV The velocity gain, in volts/(unit/sec). * @param kA The acceleration gain, in volts/(unit/sec²). - * @throws std::domain_error if kV <= 0 or kA <= 0. + * @throws std::domain_error if kV < 0 or kA <= 0. * @see https://github.com/wpilibsuite/sysid */ @@ -89,8 +89,8 @@ class WPILIB_DLLEXPORT LinearSystemId { static LinearSystem<1, 1, 1> IdentifyVelocitySystem( decltype(1_V / Velocity_t(1)) kV, decltype(1_V / Acceleration_t(1)) kA) { - if (kV <= decltype(kV){0}) { - throw std::domain_error("Kv must be greater than zero."); + if (kV < decltype(kV){0}) { + throw std::domain_error("Kv must be greater than or equal to zero."); } if (kA <= decltype(kA){0}) { throw std::domain_error("Ka must be greater than zero."); @@ -122,7 +122,7 @@ class WPILIB_DLLEXPORT LinearSystemId { * @param kV The velocity gain, in volts/(unit/sec). * @param kA The acceleration gain, in volts/(unit/sec²). * - * @throws std::domain_error if kV <= 0 or kA <= 0. + * @throws std::domain_error if kV < 0 or kA <= 0. * @see https://github.com/wpilibsuite/sysid */ @@ -132,8 +132,8 @@ class WPILIB_DLLEXPORT LinearSystemId { static LinearSystem<2, 1, 1> IdentifyPositionSystem( decltype(1_V / Velocity_t(1)) kV, decltype(1_V / Acceleration_t(1)) kA) { - if (kV <= decltype(kV){0}) { - throw std::domain_error("Kv must be greater than zero."); + if (kV < decltype(kV){0}) { + throw std::domain_error("Kv must be greater than or equal to zero."); } if (kA <= decltype(kA){0}) { throw std::domain_error("Ka must be greater than zero."); @@ -251,7 +251,7 @@ class WPILIB_DLLEXPORT LinearSystemId { * @param kV The velocity gain, in volts/(unit/sec). * @param kA The acceleration gain, in volts/(unit/sec²). * - * @throws std::domain_error if kV <= 0 or kA <= 0. + * @throws std::domain_error if kV < 0 or kA <= 0. */ template requires std::same_as || @@ -259,8 +259,8 @@ class WPILIB_DLLEXPORT LinearSystemId { static LinearSystem<2, 1, 2> DCMotorSystem( decltype(1_V / Velocity_t(1)) kV, decltype(1_V / Acceleration_t(1)) kA) { - if (kV <= decltype(kV){0}) { - throw std::domain_error("Kv must be greater than zero."); + if (kV < decltype(kV){0}) { + throw std::domain_error("Kv must be greater than or equal to zero."); } if (kA <= decltype(kA){0}) { throw std::domain_error("Ka must be greater than zero."); From 7cd4a75323a14778342363580e08f7a8cad04e18 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 10 Mar 2024 17:43:02 -0700 Subject: [PATCH 03/12] [sysid] Fix crash on negative feedforward gains (#6425) LinearSystemId's linear system factories throw on negative feedforward gains, but SysId can compute the feedback gains just fine in that case. Now we construct the system manually instead. Fixes #6423. --- .../main/native/cpp/analysis/FeedbackAnalysis.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp index 28096255e5..0a4f53a65c 100644 --- a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp +++ b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp @@ -8,7 +8,6 @@ #include #include -#include #include #include #include @@ -32,8 +31,10 @@ FeedbackGains sysid::CalculatePositionFeedbackGains( // instabilities in the LQR. if (Ka > 1E-7) { // Create a position system from our feedforward gains. - auto system = frc::LinearSystemId::IdentifyPositionSystem( - Kv_t(Kv), Ka_t(Ka)); + frc::LinearSystem<2, 1, 1> system{ + frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -Kv / Ka}}, + frc::Matrixd<2, 1>{0.0, 1.0 / Ka}, frc::Matrixd<1, 2>{1.0, 0.0}, + frc::Matrixd<1, 1>{0.0}}; // Create an LQR with 2 states to control -- position and velocity. frc::LinearQuadraticRegulator<2, 1> controller{ system, {params.qp, params.qv}, {params.r}, preset.period}; @@ -74,8 +75,9 @@ FeedbackGains sysid::CalculateVelocityFeedbackGains( } // Create a velocity system from our feedforward gains. - auto system = frc::LinearSystemId::IdentifyVelocitySystem( - Kv_t(Kv), Ka_t(Ka)); + frc::LinearSystem<1, 1, 1> system{ + frc::Matrixd<1, 1>{-Kv / Ka}, frc::Matrixd<1, 1>{1.0 / Ka}, + frc::Matrixd<1, 1>{1.0}, frc::Matrixd<1, 1>{0.0}}; // Create an LQR controller with 1 state -- velocity. frc::LinearQuadraticRegulator<1, 1> controller{ system, {params.qv}, {params.r}, preset.period}; From fbd239d15e199163717062580f94ff24b4abf27f Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Sun, 10 Mar 2024 20:29:20 -0700 Subject: [PATCH 04/12] [sim] GUI: Use shift to enable docking features (#6429) --- simulation/halsim_gui/src/main/native/cpp/main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/simulation/halsim_gui/src/main/native/cpp/main.cpp b/simulation/halsim_gui/src/main/native/cpp/main.cpp index 0cedd36978..54510239bc 100644 --- a/simulation/halsim_gui/src/main/native/cpp/main.cpp +++ b/simulation/halsim_gui/src/main/native/cpp/main.cpp @@ -51,6 +51,8 @@ __declspec(dllexport) glass::SetStorageName("simgui"); + gui::AddInit([] { ImGui::GetIO().ConfigDockingWithShift = true; }); + HAL_RegisterExtension(HALSIMGUI_EXT_ADDGUIINIT, reinterpret_cast((AddGuiInitFn)&AddGuiInit)); HAL_RegisterExtension( From 3d9152a4613b1cfbeba0a36665f6e949d2056a3d Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Sun, 10 Mar 2024 20:32:54 -0700 Subject: [PATCH 05/12] [hal] Raise SIGKILL instead of calling abort() (#6427) We don't need to generate a core dump here if core dumps are enabled. --- hal/src/main/native/cpp/jni/HAL.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/hal/src/main/native/cpp/jni/HAL.cpp b/hal/src/main/native/cpp/jni/HAL.cpp index 9918f46d6e..99e4675ea9 100644 --- a/hal/src/main/native/cpp/jni/HAL.cpp +++ b/hal/src/main/native/cpp/jni/HAL.cpp @@ -6,8 +6,11 @@ #include +#ifdef __FRC_ROBORIO__ +#include +#endif + #include -#include #include #include @@ -93,7 +96,7 @@ Java_edu_wpi_first_hal_HAL_terminate (JNIEnv*, jclass) { #ifdef __FRC_ROBORIO__ - std::abort(); + ::raise(SIGILL); #endif } From 11c60df3e02062aa06b8d57711f1a3885229967f Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Mon, 11 Mar 2024 09:43:33 -0700 Subject: [PATCH 06/12] [hal] Use SIGKILL instead of SIGILL (#6431) Fix typo. --- hal/src/main/native/cpp/jni/HAL.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hal/src/main/native/cpp/jni/HAL.cpp b/hal/src/main/native/cpp/jni/HAL.cpp index 99e4675ea9..7dcdf45edb 100644 --- a/hal/src/main/native/cpp/jni/HAL.cpp +++ b/hal/src/main/native/cpp/jni/HAL.cpp @@ -96,7 +96,7 @@ Java_edu_wpi_first_hal_HAL_terminate (JNIEnv*, jclass) { #ifdef __FRC_ROBORIO__ - ::raise(SIGILL); + ::raise(SIGKILL); #endif } From f74f6f1d429d73100b651dcfe1677c06b85c7c0b Mon Sep 17 00:00:00 2001 From: sciencewhiz Date: Mon, 11 Mar 2024 20:22:33 -0700 Subject: [PATCH 07/12] [docs] Add docs for features not supported on PDH (NFC) (#6436) --- .../wpi/first/hal/PowerDistributionJNI.java | 14 +++++++++++--- .../native/include/hal/PowerDistribution.h | 14 +++++++++++--- .../native/include/frc/PowerDistribution.h | 17 +++++++++++++---- .../wpi/first/wpilibj/PowerDistribution.java | 18 ++++++++++++++---- 4 files changed, 49 insertions(+), 14 deletions(-) diff --git a/hal/src/main/java/edu/wpi/first/hal/PowerDistributionJNI.java b/hal/src/main/java/edu/wpi/first/hal/PowerDistributionJNI.java index 93f9a3dcca..f2b096ccf7 100644 --- a/hal/src/main/java/edu/wpi/first/hal/PowerDistributionJNI.java +++ b/hal/src/main/java/edu/wpi/first/hal/PowerDistributionJNI.java @@ -83,6 +83,8 @@ public class PowerDistributionJNI extends JNIWrapper { /** * Gets the temperature of the PowerDistribution. * + *

Not supported on the Rev PDH and returns 0. + * * @param handle the module handle * @return the module temperature (celsius) * @see "HAL_GetPowerDistributionTemperature" @@ -129,7 +131,9 @@ public class PowerDistributionJNI extends JNIWrapper { public static native double getTotalCurrent(int handle); /** - * Gets the total power of the PowerDistribution. + * Gets the total power of the Power Distribution Panel. + * + *

Not supported on the Rev PDH and returns 0. * * @param handle the module handle * @return the total power (watts) @@ -138,7 +142,9 @@ public class PowerDistributionJNI extends JNIWrapper { public static native double getTotalPower(int handle); /** - * Gets the total energy of the PowerDistribution. + * Gets the total energy of the Power Distribution Panel. + * + *

Not supported on the Rev PDH and does nothing. * * @param handle the module handle * @return the total energy (joules) @@ -147,7 +153,9 @@ public class PowerDistributionJNI extends JNIWrapper { public static native double getTotalEnergy(int handle); /** - * Resets the PowerDistribution accumulated energy. + * Resets the Power Distribution Panel accumulated energy. + * + *

Not supported on the Rev PDH and returns 0. * * @param handle the module handle * @see "HAL_ClearPowerDistributionStickyFaults" diff --git a/hal/src/main/native/include/hal/PowerDistribution.h b/hal/src/main/native/include/hal/PowerDistribution.h index c2dc3cd705..835f2eaf4c 100644 --- a/hal/src/main/native/include/hal/PowerDistribution.h +++ b/hal/src/main/native/include/hal/PowerDistribution.h @@ -101,7 +101,9 @@ int32_t HAL_GetPowerDistributionNumChannels(HAL_PowerDistributionHandle handle, int32_t* status); /** - * Gets the temperature of the PowerDistribution. + * Gets the temperature of the Power Distribution Panel. + * + * Not supported on the Rev PDH and returns 0. * * @param[in] handle the module handle * @param[out] status Error status variable. 0 on success. @@ -156,7 +158,9 @@ double HAL_GetPowerDistributionTotalCurrent(HAL_PowerDistributionHandle handle, int32_t* status); /** - * Gets the total power of the PowerDistribution. + * Gets the total power of the Power Distribution Panel. + * + * Not supported on the Rev PDH and returns 0. * * @param[in] handle the module handle * @param[out] status Error status variable. 0 on success. @@ -166,7 +170,9 @@ double HAL_GetPowerDistributionTotalPower(HAL_PowerDistributionHandle handle, int32_t* status); /** - * Gets the total energy of the PowerDistribution. + * Gets the total energy of the Power Distribution Panel. + * + * Not supported on the Rev PDH and returns 0. * * @param[in] handle the module handle * @param[out] status Error status variable. 0 on success. @@ -178,6 +184,8 @@ double HAL_GetPowerDistributionTotalEnergy(HAL_PowerDistributionHandle handle, /** * Resets the PowerDistribution accumulated energy. * + * Not supported on the Rev PDH and does nothing. + * * @param[in] handle the module handle * @param[out] status Error status variable. 0 on success. */ diff --git a/wpilibc/src/main/native/include/frc/PowerDistribution.h b/wpilibc/src/main/native/include/frc/PowerDistribution.h index 4a6eb4fa18..9a219472dd 100644 --- a/wpilibc/src/main/native/include/frc/PowerDistribution.h +++ b/wpilibc/src/main/native/include/frc/PowerDistribution.h @@ -58,7 +58,10 @@ class PowerDistribution : public wpi::Sendable, double GetVoltage() const; /** - * Query the temperature of the PDP/PDH. + * Query the temperature of the PDP. + * + * Not supported on the Rev PDH and returns 0. + * * * @return The temperature in degrees Celsius */ @@ -80,21 +83,27 @@ class PowerDistribution : public wpi::Sendable, double GetTotalCurrent() const; /** - * Query the total power drawn from all monitored PDP/PDH channels. + * Query the total power drawn from all monitored PDP channels. + * + * Not supported on the Rev PDH and returns 0. * * @return The total power drawn in Watts */ double GetTotalPower() const; /** - * Query the total energy drawn from the monitored PDP/PDH channels. + * Query the total energy drawn from the monitored PDP channels. + * + * Not supported on the Rev PDH and returns 0. * * @return The total energy drawn in Joules */ double GetTotalEnergy() const; /** - * Reset the total energy drawn from the PDP/PDH. + * Reset the total energy drawn from the PDP. + * + * Not supported on the Rev PDH and does nothing. * * @see PowerDistribution#GetTotalEnergy */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java index 57709b77e1..9aa1b7c743 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java @@ -93,7 +93,9 @@ public class PowerDistribution implements Sendable, AutoCloseable { } /** - * Query the temperature of the PDP/PDH. + * Query the temperature of the PDP. + * + *

Not supported on the Rev PDH and returns 0. * * @return The temperature in degrees Celsius */ @@ -121,7 +123,9 @@ public class PowerDistribution implements Sendable, AutoCloseable { } /** - * Query the total power drawn from the monitored channels. + * Query the total power drawn from the monitored channels of the PDP. + * + *

Not supported on the Rev PDH and returns 0. * * @return the total power in Watts */ @@ -130,7 +134,9 @@ public class PowerDistribution implements Sendable, AutoCloseable { } /** - * Query the total energy drawn from the monitored channels. + * Query the total energy drawn from the monitored channels of the PDP. + * + *

Not supported on the Rev PDH and returns 0. * * @return the total energy in Joules */ @@ -138,7 +144,11 @@ public class PowerDistribution implements Sendable, AutoCloseable { return PowerDistributionJNI.getTotalEnergy(m_handle); } - /** Reset the total energy to 0. */ + /** + * Reset the total energy to 0 of the PDP. + * + *

Not supported on the Rev PDH and does nothing. + */ public void resetTotalEnergy() { PowerDistributionJNI.resetTotalEnergy(m_handle); } From 0e013dc0218628e6cab5aeab01cfd0eb37b5ce80 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 11 Mar 2024 20:23:03 -0700 Subject: [PATCH 08/12] [sysid] Fix "Sample" docs typo (NFC) (#6435) --- sysid/src/main/native/include/sysid/analysis/ArmSim.h | 2 +- sysid/src/main/native/include/sysid/analysis/ElevatorSim.h | 2 +- sysid/src/main/native/include/sysid/analysis/SimpleMotorSim.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/sysid/src/main/native/include/sysid/analysis/ArmSim.h b/sysid/src/main/native/include/sysid/analysis/ArmSim.h index af6c10e5a3..1e80b8203a 100644 --- a/sysid/src/main/native/include/sysid/analysis/ArmSim.h +++ b/sysid/src/main/native/include/sysid/analysis/ArmSim.h @@ -33,7 +33,7 @@ class ArmSim { * forward dt seconds. * * @param voltage Voltage to apply over the timestep. - * @param dt Sammple period. + * @param dt Sample period. */ void Update(units::volt_t voltage, units::second_t dt); diff --git a/sysid/src/main/native/include/sysid/analysis/ElevatorSim.h b/sysid/src/main/native/include/sysid/analysis/ElevatorSim.h index 2d0c5f68c8..82db2de7ad 100644 --- a/sysid/src/main/native/include/sysid/analysis/ElevatorSim.h +++ b/sysid/src/main/native/include/sysid/analysis/ElevatorSim.h @@ -31,7 +31,7 @@ class ElevatorSim { * dt seconds. * * @param voltage Voltage to apply over the timestep. - * @param dt Sammple period. + * @param dt Sample period. */ void Update(units::volt_t voltage, units::second_t dt); diff --git a/sysid/src/main/native/include/sysid/analysis/SimpleMotorSim.h b/sysid/src/main/native/include/sysid/analysis/SimpleMotorSim.h index a452c64062..920810ffed 100644 --- a/sysid/src/main/native/include/sysid/analysis/SimpleMotorSim.h +++ b/sysid/src/main/native/include/sysid/analysis/SimpleMotorSim.h @@ -30,7 +30,7 @@ class SimpleMotorSim { * seconds. * * @param voltage Voltage to apply over the timestep. - * @param dt Sammple period. + * @param dt Sample period. */ void Update(units::volt_t voltage, units::second_t dt); From 3116f790ea95d815a509278c077f2ffc0d175868 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Tue, 12 Mar 2024 21:49:28 -0700 Subject: [PATCH 09/12] [sysid] Fix wrong position Kd with unnormalized time (#6433) --- .../main/native/cpp/analysis/FeedbackAnalysis.cpp | 7 ++++--- .../native/cpp/analysis/FeedbackAnalysisTest.cpp | 13 +++++++++++++ 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp index 0a4f53a65c..7821812bd4 100644 --- a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp +++ b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp @@ -42,9 +42,10 @@ FeedbackGains sysid::CalculatePositionFeedbackGains( controller.LatencyCompensate(system, preset.period, preset.measurementDelay); - return {controller.K(0, 0) * preset.outputConversionFactor, - controller.K(0, 1) * preset.outputConversionFactor / - (preset.normalized ? 1 : preset.period.value())}; + return { + controller.K(0, 0) * preset.outputConversionFactor, + controller.K(0, 1) * preset.outputConversionFactor / + (preset.normalized ? 1 : units::second_t{preset.period}.value())}; } // This is our special model to avoid instabilities in the LQR. diff --git a/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp b/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp index 3e1474ef37..511950ed96 100644 --- a/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp +++ b/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp @@ -130,3 +130,16 @@ TEST(FeedbackAnalysisTest, PositionWithLatencyCompensation) { EXPECT_NEAR(Kp, 5.92, 0.05); EXPECT_NEAR(Kd, 2.12, 0.05); } + +TEST(FeedbackAnalysisTest, PositionREV) { + auto Kv = 3.060; + auto Ka = 0.327; + + sysid::LQRParameters params{1, 1.5, 7}; + + auto [Kp, Kd] = sysid::CalculatePositionFeedbackGains( + sysid::presets::kREVNEOBuiltIn, params, Kv, Ka); + + EXPECT_NEAR(Kp, 0.30202, 0.05); + EXPECT_NEAR(Kd, 48.518, 0.05); +} From a1af2357e8dd9689f1812290b23f1270a6ea989c Mon Sep 17 00:00:00 2001 From: Sam Richter <60528506+S1ink@users.noreply.github.com> Date: Fri, 15 Mar 2024 13:50:30 -0500 Subject: [PATCH 10/12] [ntcore] Fix memory leak in WebSocketConnection (#6439) --- ntcore/src/main/native/cpp/net/WebSocketConnection.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ntcore/src/main/native/cpp/net/WebSocketConnection.cpp b/ntcore/src/main/native/cpp/net/WebSocketConnection.cpp index 176770db7f..1312af2eb2 100644 --- a/ntcore/src/main/native/cpp/net/WebSocketConnection.cpp +++ b/ntcore/src/main/native/cpp/net/WebSocketConnection.cpp @@ -233,6 +233,8 @@ int WebSocketConnection::Flush() { int count = 0; for (auto&& frame : wpi::take_back(std::span{m_frames}, unsentFrames.size())) { + ReleaseBufs( + std::span{m_bufs}.subspan(frame.start, frame.end - frame.start)); count += frame.count; } m_frames.clear(); From 0b13459469505341bbc98ae59a2e978f1cd4c0a9 Mon Sep 17 00:00:00 2001 From: person4268 <28717044+person4268@users.noreply.github.com> Date: Fri, 15 Mar 2024 15:05:48 -0400 Subject: [PATCH 11/12] [commands] Trigger: pass m_loop to new Trigger in composition functions (#6441) --- .../edu/wpi/first/wpilibj2/command/button/Trigger.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java index 5bdee377b4..46aa5e2c1d 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java @@ -237,7 +237,7 @@ public class Trigger implements BooleanSupplier { * @return A trigger which is active when both component triggers are active. */ public Trigger and(BooleanSupplier trigger) { - return new Trigger(() -> m_condition.getAsBoolean() && trigger.getAsBoolean()); + return new Trigger(m_loop, () -> m_condition.getAsBoolean() && trigger.getAsBoolean()); } /** @@ -247,7 +247,7 @@ public class Trigger implements BooleanSupplier { * @return A trigger which is active when either component trigger is active. */ public Trigger or(BooleanSupplier trigger) { - return new Trigger(() -> m_condition.getAsBoolean() || trigger.getAsBoolean()); + return new Trigger(m_loop, () -> m_condition.getAsBoolean() || trigger.getAsBoolean()); } /** @@ -257,7 +257,7 @@ public class Trigger implements BooleanSupplier { * @return the negated trigger */ public Trigger negate() { - return new Trigger(() -> !m_condition.getAsBoolean()); + return new Trigger(m_loop, () -> !m_condition.getAsBoolean()); } /** @@ -281,6 +281,7 @@ public class Trigger implements BooleanSupplier { */ public Trigger debounce(double seconds, Debouncer.DebounceType type) { return new Trigger( + m_loop, new BooleanSupplier() { final Debouncer m_debouncer = new Debouncer(seconds, type); From c10f7d91b7b4795067424df0e15911210850145e Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Sat, 16 Mar 2024 11:06:01 -0700 Subject: [PATCH 12/12] [ci] Work around asan actions bug (#6442) --- .github/workflows/sanitizers.yml | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/.github/workflows/sanitizers.yml b/.github/workflows/sanitizers.yml index cf4d1089f7..6cd7a9a407 100644 --- a/.github/workflows/sanitizers.yml +++ b/.github/workflows/sanitizers.yml @@ -26,8 +26,13 @@ jobs: ctest-flags: "" name: "${{ matrix.name }}" runs-on: ubuntu-22.04 - container: wpilib/roborio-cross-ubuntu:2024-22.04 + container: + image: wpilib/roborio-cross-ubuntu:2024-22.04 + options: --privileged steps: + - name: mmap rnd_bits workaround + run: sudo sysctl -w vm.mmap_rnd_bits=28 + - name: Install Dependencies run: sudo apt-get update && sudo apt-get install -y libopencv-dev libopencv4.5-java python-is-python3 clang-14 libprotobuf-dev protobuf-compiler ninja-build