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.RawEncoder;
import java.lang.Math;
import java.util.Arrays;
import java.util.LinkedList;
import java.util.List;
@ -199,11 +200,17 @@ public final class MecanumDrive {
}
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 {

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) {
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);
}
}