missing
8cdd3dd447
most important stuff: kinematics, autons, config, dashboard, easyopencv + april tags, and probably more im forgetting https://xkcd.com/1296/
44 lines
1.4 KiB
Java
44 lines
1.4 KiB
Java
package org.firstinspires.ftc.teamcode;
|
|
|
|
public class MotorPowers {
|
|
public final double fl;
|
|
public final double fr;
|
|
public final double bl;
|
|
public final double br;
|
|
|
|
public MotorPowers(double fl, double fr, double bl, double br) {
|
|
this.fl = fl;
|
|
this.fr = fr;
|
|
this.bl = bl;
|
|
this.br = br;
|
|
}
|
|
|
|
public static MotorPowers calculateUnscaled(double powerX, double powerY, double turn, double speed) {
|
|
double fl = (powerY + powerX + turn) * speed;
|
|
double fr = (powerY - powerX - turn) * speed;
|
|
double bl = (powerY - powerX + turn) * speed;
|
|
double br = (powerY + powerX - turn) * speed;
|
|
|
|
return new MotorPowers(fl, fr, bl, br);
|
|
}
|
|
|
|
public MotorPowers scaled() {
|
|
double scale = 1;
|
|
scale = Math.max(scale, Math.abs(this.fl));
|
|
scale = Math.max(scale, Math.abs(this.fr));
|
|
scale = Math.max(scale, Math.abs(this.bl));
|
|
scale = Math.max(scale, Math.abs(this.br));
|
|
|
|
double fl = this.fl / scale;
|
|
double fr = this.fr / scale;
|
|
double bl = this.bl / scale;
|
|
double br = this.br / scale;
|
|
|
|
return new MotorPowers(fl, fr, bl, br);
|
|
}
|
|
|
|
public static MotorPowers calculate(double powerX, double powerY, double turn, double speed) {
|
|
return MotorPowers.calculateUnscaled(powerX, powerY, turn, speed).scaled();
|
|
}
|
|
}
|