SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Fix drive powers

Browse Source
This commit is contained in:
Ryan Brott
2023-03-02 00:23:39 -08:00
parent 80ea431b29
commit 98cbe1e950
2 changed files with 21 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
View File

@ -41,6 +41,7 @@ import org.firstinspires.ftc.teamcode.util.LynxFirmwareVersion;
import org.firstinspires.ftc.teamcode.util.OverflowEncoder; import org.firstinspires.ftc.teamcode.util.OverflowEncoder;
import org.firstinspires.ftc.teamcode.util.RawEncoder; import org.firstinspires.ftc.teamcode.util.RawEncoder;
import java.lang.Math;
import java.util.Arrays; import java.util.Arrays;
import java.util.LinkedList; import java.util.LinkedList;
import java.util.List; import java.util.List;
@ -199,11 +200,17 @@ public final class MecanumDrive {
} }
public void setDrivePowers(Twist2d powers) { public void setDrivePowers(Twist2d powers) {
MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(Twist2dDual.constant(powers, 1)); MecanumKinematics.WheelVelocities<Time> wheelVels = new MecanumKinematics(1).inverse(Twist2dDual.constant(powers, 1));
leftFront.setPower(wheelVels.leftFront.get(0));
leftBack.setPower(wheelVels.leftBack.get(0)); double maxPowerMag = 1;
rightBack.setPower(wheelVels.rightBack.get(0)); for (DualNum<Time> power : wheelVels.all()) {
rightFront.setPower(wheelVels.rightFront.get(0)); 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 { public final class FollowTrajectoryAction implements Action {

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
View File

@ -204,12 +204,18 @@ public final class TankDrive {
} }
public void setDrivePowers(Twist2d powers) { 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) { for (DcMotorEx m : leftMotors) {
m.setPower(wheelVels.left.get(0)); m.setPower(wheelVels.left.get(0) / maxPowerMag);
} }
for (DcMotorEx m : rightMotors) { for (DcMotorEx m : rightMotors) {
m.setPower(wheelVels.right.get(0)); m.setPower(wheelVels.right.get(0) / maxPowerMag);
} }
} }