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

Re-create feedforward objects

Browse Source
This commit is contained in:
Ryan Brott
2023-10-09 22:07:21 -07:00
parent 2584ce8d45
commit 1b1197db0c
2 changed files with 5 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -81,8 +81,6 @@ public final class MecanumDrive {
public final MecanumKinematics kinematics = new MecanumKinematics( public final MecanumKinematics kinematics = new MecanumKinematics(
PARAMS.inPerTick * PARAMS.trackWidthTicks, PARAMS.inPerTick / PARAMS.lateralInPerTick); PARAMS.inPerTick * PARAMS.trackWidthTicks, PARAMS.inPerTick / PARAMS.lateralInPerTick);
public final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS, PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
public final TurnConstraints defaultTurnConstraints = new TurnConstraints( public final TurnConstraints defaultTurnConstraints = new TurnConstraints(
PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel); PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel);
public final VelConstraint defaultVelConstraint = public final VelConstraint defaultVelConstraint =
@ -266,6 +264,8 @@ public final class MecanumDrive {
MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command); MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
double voltage = voltageSensor.getVoltage(); double voltage = voltageSensor.getVoltage();
final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS, PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
leftFront.setPower(feedforward.compute(wheelVels.leftFront) / voltage); leftFront.setPower(feedforward.compute(wheelVels.leftFront) / voltage);
leftBack.setPower(feedforward.compute(wheelVels.leftBack) / voltage); leftBack.setPower(feedforward.compute(wheelVels.leftBack) / voltage);
rightBack.setPower(feedforward.compute(wheelVels.rightBack) / voltage); rightBack.setPower(feedforward.compute(wheelVels.rightBack) / voltage);
@ -347,6 +347,7 @@ public final class MecanumDrive {
MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command); MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
double voltage = voltageSensor.getVoltage(); double voltage = voltageSensor.getVoltage();
final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS, PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
leftFront.setPower(feedforward.compute(wheelVels.leftFront) / voltage); leftFront.setPower(feedforward.compute(wheelVels.leftFront) / voltage);
leftBack.setPower(feedforward.compute(wheelVels.leftBack) / voltage); leftBack.setPower(feedforward.compute(wheelVels.leftBack) / voltage);
rightBack.setPower(feedforward.compute(wheelVels.rightBack) / voltage); rightBack.setPower(feedforward.compute(wheelVels.rightBack) / voltage);

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

@ -83,8 +83,6 @@ public final class TankDrive {
public final TankKinematics kinematics = new TankKinematics(PARAMS.inPerTick * PARAMS.trackWidthTicks); public final TankKinematics kinematics = new TankKinematics(PARAMS.inPerTick * PARAMS.trackWidthTicks);
public final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS, PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
public final TurnConstraints defaultTurnConstraints = new TurnConstraints( public final TurnConstraints defaultTurnConstraints = new TurnConstraints(
PARAMS.maxAngVel, -PARAMS.maxAngVel, PARAMS.maxAngAccel); PARAMS.maxAngVel, -PARAMS.maxAngVel, PARAMS.maxAngAccel);
public final VelConstraint defaultVelConstraint = public final VelConstraint defaultVelConstraint =
@ -275,6 +273,7 @@ public final class TankDrive {
TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command); TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
double voltage = voltageSensor.getVoltage(); double voltage = voltageSensor.getVoltage();
final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS, PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
for (DcMotorEx m : leftMotors) { for (DcMotorEx m : leftMotors) {
m.setPower(feedforward.compute(wheelVels.left) / voltage); m.setPower(feedforward.compute(wheelVels.left) / voltage);
} }
@ -362,6 +361,7 @@ public final class TankDrive {
TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command); TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
double voltage = voltageSensor.getVoltage(); double voltage = voltageSensor.getVoltage();
final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS, PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
for (DcMotorEx m : leftMotors) { for (DcMotorEx m : leftMotors) {
m.setPower(feedforward.compute(wheelVels.left) / voltage); m.setPower(feedforward.compute(wheelVels.left) / voltage);
} }