Fix drive powers

wheelerrobotics · Mar 2, 2023 · 98cbe1e · 98cbe1e
1 parent 80ea431
commit 98cbe1e
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 8 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
@@ -41,6 +41,7 @@
 import org.firstinspires.ftc.teamcode.util.OverflowEncoder;
 import org.firstinspires.ftc.teamcode.util.RawEncoder;
 
+import java.lang.Math;
 import java.util.Arrays;
 import java.util.LinkedList;
 import java.util.List;
@@ -199,11 +200,17 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) {
     }
 
     public void setDrivePowers(Twist2d powers) {
-        MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(Twist2dDual.constant(powers, 1));
-        leftFront.setPower(wheelVels.leftFront.get(0));
-        leftBack.setPower(wheelVels.leftBack.get(0));
-        rightBack.setPower(wheelVels.rightBack.get(0));
-        rightFront.setPower(wheelVels.rightFront.get(0));
+        MecanumKinematics.WheelVelocities<Time> wheelVels = new MecanumKinematics(1).inverse(Twist2dDual.constant(powers, 1));
+
+        double maxPowerMag = 1;
+        for (DualNum<Time> power : wheelVels.all()) {
+            maxPowerMag = Math.max(maxPowerMag, power.value());
+        }
+
+        leftFront.setPower(wheelVels.leftFront.get(0) / maxPowerMag);
+        leftBack.setPower(wheelVels.leftBack.get(0) / maxPowerMag);
+        rightBack.setPower(wheelVels.rightBack.get(0) / maxPowerMag);
+        rightFront.setPower(wheelVels.rightFront.get(0) / maxPowerMag);
     }
 
     public final class FollowTrajectoryAction implements Action {

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
@@ -204,12 +204,18 @@ public TankDrive(HardwareMap hardwareMap, Pose2d pose) {
     }
 
     public void setDrivePowers(Twist2d powers) {
-        TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(Twist2dDual.constant(powers, 1));
+        TankKinematics.WheelVelocities<Time> wheelVels = new TankKinematics(2).inverse(Twist2dDual.constant(powers, 1));
+
+        double maxPowerMag = 1;
+        for (DualNum<Time> power : wheelVels.all()) {
+            maxPowerMag = Math.max(maxPowerMag, power.value());
+        }
+
         for (DcMotorEx m : leftMotors) {
-            m.setPower(wheelVels.left.get(0));
+            m.setPower(wheelVels.left.get(0) / maxPowerMag);
         }
         for (DcMotorEx m : rightMotors) {
-            m.setPower(wheelVels.right.get(0));
+            m.setPower(wheelVels.right.get(0) / maxPowerMag);
         }
     }