From ba22ca9239a73223f33a95f144ce38bb4286108f Mon Sep 17 00:00:00 2001
From: DanPeled <98838880+DanPeled@users.noreply.github.com>
Date: Wed, 28 Aug 2024 18:45:17 +0300
Subject: [PATCH] Swerve angle offset feature (#59)

Added an angle offset feature for the YAGSL swerve widget due to
different ways libraries wirk and where the robot points at 0deg

---------

Co-authored-by: Gold87 <91761103+Gold872@users.noreply.github.com>
---
 .../multi-topic/yagsl_swerve_drive.dart       | 41 ++++++++++++++++++-
 .../multi-topic/yagsl_swerve_drive_test.dart  |  3 ++
 2 files changed, 42 insertions(+), 2 deletions(-)

diff --git a/lib/widgets/nt_widgets/multi-topic/yagsl_swerve_drive.dart b/lib/widgets/nt_widgets/multi-topic/yagsl_swerve_drive.dart
index 29b436a4..d42c355d 100644
--- a/lib/widgets/nt_widgets/multi-topic/yagsl_swerve_drive.dart
+++ b/lib/widgets/nt_widgets/multi-topic/yagsl_swerve_drive.dart
@@ -6,6 +6,8 @@ import 'package:dot_cast/dot_cast.dart';
 import 'package:provider/provider.dart';
 import 'package:vector_math/vector_math_64.dart' show radians;
 
+import 'package:elastic_dashboard/services/text_formatter_builder.dart';
+import 'package:elastic_dashboard/widgets/dialog_widgets/dialog_text_input.dart';
 import 'package:elastic_dashboard/widgets/dialog_widgets/dialog_toggle_switch.dart';
 import 'package:elastic_dashboard/widgets/nt_widgets/nt_widget.dart';
 
@@ -23,31 +25,42 @@ class YAGSLSwerveDriveModel extends NTWidgetModel {
 
   bool _showRobotRotation = true;
   bool _showDesiredStates = true;
+  double _angleOffset =
+      0; // Modifiable angle offset to allow all kinds of swerve libraries setups
 
-  get showRobotRotation => _showRobotRotation;
+  bool get showRobotRotation => _showRobotRotation;
 
   set showRobotRotation(value) {
     _showRobotRotation = value;
     refresh();
   }
 
-  get showDesiredStates => _showDesiredStates;
+  bool get showDesiredStates => _showDesiredStates;
 
   set showDesiredStates(value) {
     _showDesiredStates = value;
     refresh();
   }
 
+  double get angleOffset => _angleOffset;
+
+  set angleOffset(double value) {
+    _angleOffset = value;
+    refresh();
+  }
+
   YAGSLSwerveDriveModel({
     required super.ntConnection,
     required super.preferences,
     required super.topic,
     bool showRobotRotation = true,
     bool showDesiredStates = true,
+    double angleOffset = 0.0,
     super.dataType,
     super.period,
   })  : _showDesiredStates = showDesiredStates,
         _showRobotRotation = showRobotRotation,
+        _angleOffset = angleOffset,
         super();
 
   YAGSLSwerveDriveModel.fromJson({
@@ -57,6 +70,7 @@ class YAGSLSwerveDriveModel extends NTWidgetModel {
   }) : super.fromJson(jsonData: jsonData) {
     _showRobotRotation = tryCast(jsonData['show_robot_rotation']) ?? true;
     _showDesiredStates = tryCast(jsonData['show_desired_states']) ?? true;
+    _angleOffset = tryCast(jsonData['angle_offset']) ?? 0.0;
   }
 
   @override
@@ -65,6 +79,7 @@ class YAGSLSwerveDriveModel extends NTWidgetModel {
       ...super.toJson(),
       'show_robot_rotation': _showRobotRotation,
       'show_desired_states': _showDesiredStates,
+      'angle_offset': _angleOffset,
     };
   }
 
@@ -93,6 +108,26 @@ class YAGSLSwerveDriveModel extends NTWidgetModel {
           ),
         ],
       ),
+      const SizedBox(height: 5),
+      Row(
+        children: [
+          Flexible(
+            child: DialogTextInput(
+              initialText: angleOffset.toString(),
+              label: 'Angle Offset (degrees)',
+              onSubmit: (String value) {
+                double? doubleValue = double.tryParse(value);
+
+                if (doubleValue != null) {
+                  angleOffset = doubleValue;
+                }
+              },
+              formatter: TextFormatterBuilder.decimalTextFormatter(
+                  allowNegative: true),
+            ),
+          ),
+        ],
+      ),
     ];
   }
 
@@ -189,6 +224,8 @@ class YAGSLSwerveDrive extends NTWidget {
           robotAngle *= 2 * pi;
         }
 
+        robotAngle -= radians(model.angleOffset);
+
         double maxSpeed = tryCast(model.ntConnection
                 .getLastAnnouncedValue(model.maxSpeedTopic)) ??
             4.5;
diff --git a/test/widgets/nt_widgets/multi-topic/yagsl_swerve_drive_test.dart b/test/widgets/nt_widgets/multi-topic/yagsl_swerve_drive_test.dart
index 8d770c1e..cc3abd01 100644
--- a/test/widgets/nt_widgets/multi-topic/yagsl_swerve_drive_test.dart
+++ b/test/widgets/nt_widgets/multi-topic/yagsl_swerve_drive_test.dart
@@ -18,6 +18,7 @@ void main() {
     'period': 0.100,
     'show_robot_rotation': true,
     'show_desired_states': true,
+    'angle_offset': 90.0,
   };
 
   late SharedPreferences preferences;
@@ -47,6 +48,7 @@ void main() {
 
     expect(yagslSwerveModel.showRobotRotation, isTrue);
     expect(yagslSwerveModel.showDesiredStates, isTrue);
+    expect(yagslSwerveModel.angleOffset, 90.0);
   });
 
   test('YAGSL swerve drive to json', () {
@@ -57,6 +59,7 @@ void main() {
       period: 0.100,
       showRobotRotation: true,
       showDesiredStates: true,
+      angleOffset: 90.0,
     );
 
     expect(yagslSwerveModel.toJson(), yagslSwerveJson);