diff --git a/src/main/java/frc/libzodiac/Constant.java b/src/main/java/frc/libzodiac/Constant.java new file mode 100644 index 0000000..2c37088 --- /dev/null +++ b/src/main/java/frc/libzodiac/Constant.java @@ -0,0 +1,14 @@ +package frc.libzodiac; + +public final class Constant { + // 2023.6.24: going to measure + /** + * How many radians a raw TalonSRX's unit is equal to. + */ + public static double TALONSRX_ENCODER_UNIT = 2 * Math.PI / 4096; + + /** + * Falcon uses rotation as its position unit. + */ + public static double FALCON_POSITION_UNIT = 2 * Math.PI; +} diff --git a/src/main/java/frc/libzodiac/Util.java b/src/main/java/frc/libzodiac/Util.java index 4be8bc4..c3bafa0 100644 --- a/src/main/java/frc/libzodiac/Util.java +++ b/src/main/java/frc/libzodiac/Util.java @@ -21,7 +21,7 @@ public static double deg(double rad) { /** * Extends the modulo operation to R. *

- * THe definition here: + * The definition here: *

* a and b are congruent modulo c, if and only if (a-b)/c is integer. * diff --git a/src/main/java/frc/libzodiac/ZEncoder.java b/src/main/java/frc/libzodiac/ZEncoder.java index 9de1da7..ae6960f 100644 --- a/src/main/java/frc/libzodiac/ZEncoder.java +++ b/src/main/java/frc/libzodiac/ZEncoder.java @@ -11,9 +11,9 @@ public abstract class ZEncoder { public final int can_id; /** - * Zero position of this encoder in raw units. + * Zero position of this encoder in radians. The adjustion is applied to output automatically by ZEncoder. */ - public double zero_pos; + public double zero; public ZEncoder(int can_id) { this.can_id = can_id; @@ -28,7 +28,7 @@ public ZEncoder(int can_id) { * Configures the zero position of the encoder. */ public ZEncoder set_zero(double pos) { - this.zero_pos = pos; + this.zero = pos; return this; } @@ -37,8 +37,15 @@ public ZEncoder set_zero(double pos) { */ public abstract double get_raw(); + /** + * Get the current position in radians, regardless of zero position. + */ + public abstract double get_rad(); + /** * Get the current position. */ - public abstract double get(); + public double get() { + return this.get_rad() - this.zero; + } } diff --git a/src/main/java/frc/libzodiac/ZGyro.java b/src/main/java/frc/libzodiac/ZGyro.java index dfd7056..11b75db 100644 --- a/src/main/java/frc/libzodiac/ZGyro.java +++ b/src/main/java/frc/libzodiac/ZGyro.java @@ -1,7 +1,7 @@ package frc.libzodiac; public abstract class ZGyro { - protected final int can_id; + public final int can_id; /** * Zero position in raw sensor units. diff --git a/src/main/java/frc/libzodiac/ZMotor.java b/src/main/java/frc/libzodiac/ZMotor.java index 27250aa..da54b07 100644 --- a/src/main/java/frc/libzodiac/ZMotor.java +++ b/src/main/java/frc/libzodiac/ZMotor.java @@ -1,9 +1,10 @@ package frc.libzodiac; -import java.util.HashMap; - import frc.libzodiac.util.PIDProfile; +import java.util.HashMap; +import java.util.Objects; + /** * Defines a large collection of APIs to operate various motors so that motors * can be controled under a unified generic way. @@ -20,18 +21,6 @@ public abstract class ZMotor { */ public HashMap profile; - /** - * Gets the value of the specified motion profile. - */ - protected double profile(String name) { - var opt = this.profile.get(name); - if (opt == null) { - return 0; - } else { - return opt; - } - } - /** * Initialize this motor, e.g. binds to the CAN bus. */ @@ -45,10 +34,12 @@ protected double profile(String name) { /** * Set PID parameters. */ - public ZMotor set_pid(PIDProfile pid) { + public final ZMotor set_pid(PIDProfile pid) { this.pid = pid; return this.apply_pid(); - }; + } + + ; /** * Set PID parameters. @@ -75,7 +66,7 @@ public ZMotor set_pid(double k_p, double k_i, double k_d) { /** * Set the output. - * + * * @param raw_unit Usually speed output percentage in [-1,1] for generic motors * and desired angle positions in rads for servos. */ diff --git a/src/main/java/frc/libzodiac/Zoystick.java b/src/main/java/frc/libzodiac/Zoystick.java index 1f5d527..10d4ceb 100644 --- a/src/main/java/frc/libzodiac/Zoystick.java +++ b/src/main/java/frc/libzodiac/Zoystick.java @@ -69,14 +69,14 @@ public double axis(int axis) { } /** - * Gets the x axis of the joystick. + * Gets the x-axis of the joystick. */ public double x() { return this.axis(0); } /** - * Gets the y axis of the joystick. + * Gets the y-axis of the joystick. */ public double y() { return this.axis(1); @@ -85,7 +85,7 @@ public double y() { /** * Set the input filter function. */ - public Zoystick with_filter(Function f) { + public Zoystick set_filter(Function f) { this.filter_fn = f; return this; } diff --git a/src/main/java/frc/libzodiac/Zwerve.java b/src/main/java/frc/libzodiac/Zwerve.java index 5e6e241..b1540ae 100644 --- a/src/main/java/frc/libzodiac/Zwerve.java +++ b/src/main/java/frc/libzodiac/Zwerve.java @@ -12,27 +12,13 @@ */ public abstract class Zwerve extends SubsystemBase { - /** - * The length of the bot. - *

- * Units are not that important since the program only uses the ratio of length - * and width. - */ - private final double length; - - /** - * The width of the bot. - *

- * Units are not that important since the program only uses the ratio of length - * and width. - */ - private final double width; + public final Vec2D shape; /** * Method to calculate the radius of the rectangular robot. */ private double radius() { - return Math.hypot(this.length, this.width) / 2; + return this.shape.div(2).r(); } public boolean headless = false; @@ -115,13 +101,14 @@ public static interface Module { /** * Creates a new Zwerve. * - * @param modules See Zwerve.module + * @param modules See Zwerve.module. + * @param gyro The gyro. + * @param shape Shape of the robot, x for length and y for width. */ - public Zwerve(Module[] modules, ZGyro gyro, double length, double width) { + public Zwerve(Module[] modules, ZGyro gyro, Vec2D shape) { this.module = modules; this.gyro = gyro; - this.length = length; - this.width = width; + this.shape = shape; } /** @@ -142,11 +129,12 @@ public Zwerve init() { /** * Kinematics part from 6941. */ + @Deprecated public Zwerve go_previous(Vec2D velocity, double omega) { var x = velocity.x; var y = velocity.y; - var l = this.length; - var w = this.width; + var l = this.shape.x; + var w = this.shape.y; var r = this.radius(); var a = y - omega * l / r; var b = y + omega * l / r; @@ -174,8 +162,8 @@ public Zwerve go_previous(Vec2D velocity, double omega) { * Kinematics part rewritten using vector calculations. */ public Zwerve go(Vec2D velocity, double omega) { - var l = this.length / 2; - var w = this.width / 2; + var l = this.shape.x / 2; + var w = this.shape.y / 2; var pi2 = Math.PI / 2; var vt = omega * this.radius(); Vec2D[] v = { @@ -184,10 +172,8 @@ public Zwerve go(Vec2D velocity, double omega) { new Vec2D(-l, -w).rot(pi2).with_r(vt).add(velocity), new Vec2D(l, -w).rot(pi2).with_r(vt).add(velocity), }; - var max = v[1].max(v[2]).max(v[3]).max(v[4]).r(); - if (max > 1) - for (var i : v) - i = i.div(max); + var max = v[0].max(v[1]).max(v[2]).max(v[3]).r(); + if (max > 1) for (int i = 0; i < 4; i++) v[i] = v[i].div(max); for (int i = 0; i < 4; i++) this.module[i].go(v[i].mul(output).rot(this.dir_fix())); return this; @@ -264,12 +250,6 @@ protected Drive exec() { v_y = -v_y; if (this.inv_rot) omega = -omega; - if (v_x < 1e-3) - v_x = 0; - if (v_y < 1e-3) - v_y = 0; - if (omega < 1e-3) - omega = 0; this.chassis.go(new Vec2D(v_x, v_y), omega); return this; } @@ -302,4 +282,5 @@ public Drive inv(boolean v_x, boolean v_y, boolean rot) { public Drive control(Zoystick vel, Zoystick rot) { return new Drive(this, vel, rot); } + } diff --git a/src/main/java/frc/libzodiac/hardware/Falcon.java b/src/main/java/frc/libzodiac/hardware/Falcon.java index 2b40869..0a687bd 100644 --- a/src/main/java/frc/libzodiac/hardware/Falcon.java +++ b/src/main/java/frc/libzodiac/hardware/Falcon.java @@ -3,9 +3,10 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.controls.StaticBrake; import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; - +import frc.libzodiac.Constant; import frc.libzodiac.ZMotor; import frc.libzodiac.Zervo; @@ -48,13 +49,13 @@ public Falcon shutdown() { @Override public Falcon stop() { - this.motor.stopMotor(); + this.motor.setControl(new StaticBrake()); return this; } @Override public Falcon go(String profile) { - this.motor.setControl(new VelocityDutyCycle(this.profile(profile))); + this.motor.setControl(new VelocityDutyCycle(this.profile.get(profile))); return this; } @@ -66,6 +67,9 @@ public Falcon go(double raw_unit) { public static class Servo extends Falcon implements Zervo { + /** + * Zero position applied to adjust output value of Falcon's builtin encoder. + */ protected double zero = 0; public Servo(int can_id) { @@ -74,13 +78,13 @@ public Servo(int can_id) { @Override public Servo go(String profile) { - this.motor.setControl(new PositionDutyCycle(this.profile(profile))); + this.motor.setControl(new PositionDutyCycle(this.profile.get(profile))); return this; } @Override - public Servo go(double rad) { - this.motor.setControl(new PositionDutyCycle(rad / 2 / Math.PI)); + public Servo go(double angle) { + this.motor.setControl(new PositionDutyCycle((angle - this.zero) / Constant.FALCON_POSITION_UNIT)); return this; } @@ -97,7 +101,7 @@ public double get_zero() { @Override public double get_pos() { - return this.motor.getPosition().getValue() * 2 * Math.PI - this.zero; + return this.motor.getPosition().refresh().getValue() * Constant.FALCON_POSITION_UNIT - this.zero; } @Override diff --git a/src/main/java/frc/libzodiac/hardware/Pigeon.java b/src/main/java/frc/libzodiac/hardware/Pigeon.java index 7cd67f3..83766fb 100644 --- a/src/main/java/frc/libzodiac/hardware/Pigeon.java +++ b/src/main/java/frc/libzodiac/hardware/Pigeon.java @@ -21,17 +21,17 @@ public Pigeon init() { @Override protected double get_yaw() { - return Util.rad(this.gyro.getYaw().getValue()); + return Util.rad(this.gyro.getYaw().refresh().getValue()); } @Override protected double get_pitch() { - return Util.rad(this.gyro.getPitch().getValue()); + return Util.rad(this.gyro.getPitch().refresh().getValue()); } @Override protected double get_roll() { - return Util.rad(this.gyro.getRoll().getValue()); + return Util.rad(this.gyro.getRoll().refresh().getValue()); } } diff --git a/src/main/java/frc/libzodiac/hardware/TalonSRXEncoder.java b/src/main/java/frc/libzodiac/hardware/TalonSRXEncoder.java index bf80230..3f0eb8d 100644 --- a/src/main/java/frc/libzodiac/hardware/TalonSRXEncoder.java +++ b/src/main/java/frc/libzodiac/hardware/TalonSRXEncoder.java @@ -1,10 +1,11 @@ package frc.libzodiac.hardware; import com.ctre.phoenix.motorcontrol.can.TalonSRX; - +import frc.libzodiac.Constant; import frc.libzodiac.ZEncoder; public class TalonSRXEncoder extends ZEncoder { + public TalonSRXEncoder(int can_id) { super(can_id); } @@ -23,9 +24,8 @@ public double get_raw() { } @Override - public double get() { - // TODO - return this.get_raw(); + public double get_rad() { + return this.get_raw() / Constant.TALONSRX_ENCODER_UNIT; } } diff --git a/src/main/java/frc/libzodiac/hardware/TalonSRXMotor.java b/src/main/java/frc/libzodiac/hardware/TalonSRXMotor.java index 0450d7f..b85b096 100644 --- a/src/main/java/frc/libzodiac/hardware/TalonSRXMotor.java +++ b/src/main/java/frc/libzodiac/hardware/TalonSRXMotor.java @@ -44,7 +44,7 @@ public ZMotor stop() { @Override public ZMotor go(String profile) { - this.motor.set(ControlMode.Velocity, this.profile(profile)); + this.motor.set(ControlMode.Velocity, this.profile.get(profile)); return this; } @@ -80,7 +80,7 @@ public double get_pos() { @Override public Servo go(String profile) { - this.motor.set(ControlMode.Position, this.profile(profile)); + this.motor.set(ControlMode.Position, this.profile.get(profile)); return this; } diff --git a/src/main/java/frc/libzodiac/hardware/group/FalconSwerve.java b/src/main/java/frc/libzodiac/hardware/group/FalconSwerve.java index ac3c4d4..61ef585 100644 --- a/src/main/java/frc/libzodiac/hardware/group/FalconSwerve.java +++ b/src/main/java/frc/libzodiac/hardware/group/FalconSwerve.java @@ -7,11 +7,11 @@ public class FalconSwerve implements Module { public final Falcon speed_motor; - public final Falcon.Servo angle_motor; + public final FalconWithEncoder angle_motor; - public FalconSwerve(int speed_motor_id, int angle_motor_id) { - this.speed_motor = new Falcon(speed_motor_id); - this.angle_motor = new Falcon.Servo(angle_motor_id); + public FalconSwerve(Falcon speed_motor, FalconWithEncoder angle_motor) { + this.speed_motor = speed_motor; + this.angle_motor = angle_motor; } @Override diff --git a/src/main/java/frc/libzodiac/hardware/group/FalconWithEncoder.java b/src/main/java/frc/libzodiac/hardware/group/FalconWithEncoder.java new file mode 100644 index 0000000..467e049 --- /dev/null +++ b/src/main/java/frc/libzodiac/hardware/group/FalconWithEncoder.java @@ -0,0 +1,34 @@ +package frc.libzodiac.hardware.group; + +import frc.libzodiac.ZEncoder; +import frc.libzodiac.hardware.Falcon; + +/** + * Falcon's built-in encoder resets its zero position to the position when power-on. + * Therefore, a TalonSRXEncoder is used so that it is able to control the servo with absolute positions. + */ +public class FalconWithEncoder extends Falcon.Servo { + protected final ZEncoder encoder; + + public FalconWithEncoder(int motor_can_id, ZEncoder encoder) { + super(motor_can_id); + this.encoder = encoder; + } + + @Override + public FalconWithEncoder init() { + super.init(); + this.encoder.init(); + return this; + } + + /** + * Set the zero position of this motor. Note that the position is relative to the absolute encoder, note Falcon. + */ + @Override + public FalconWithEncoder set_zero(double abs_zero) { + this.encoder.zero = abs_zero; + this.zero += this.get_pos() - this.encoder.get(); + return this; + } +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 33d2d7a..2815dc8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -32,11 +32,11 @@ public Command chassis_ctrl() { public Zoystick drive = new Zoystick(0) .map(0, "X") .map(1, "Y") - .with_filter(Zoystick.quad_filter); + .set_filter(Zoystick.quad_filter); public Zoystick ctrl = new Zoystick(1) .map(0, "X") .map(1, "Y") - .with_filter(Zoystick.thre_filter(0.1)); + .set_filter(Zoystick.thre_filter(0.1)); public RobotContainer init() { chassis.init(); diff --git a/src/main/java/frc/robot/subsystems/Chassis.java b/src/main/java/frc/robot/subsystems/Chassis.java index ce249f2..b144604 100644 --- a/src/main/java/frc/robot/subsystems/Chassis.java +++ b/src/main/java/frc/robot/subsystems/Chassis.java @@ -4,17 +4,43 @@ package frc.robot.subsystems; +import frc.libzodiac.ZEncoder; import frc.libzodiac.Zwerve; +import frc.libzodiac.hardware.Falcon; import frc.libzodiac.hardware.Pigeon; +import frc.libzodiac.hardware.TalonSRXEncoder; import frc.libzodiac.hardware.group.FalconSwerve; +import frc.libzodiac.hardware.group.FalconWithEncoder; +import frc.libzodiac.util.Vec2D; public class Chassis extends Zwerve { + public static final Falcon[] speed_motor = { + new Falcon(1), + new Falcon(2), + new Falcon(3), + new Falcon(4) + }; + + public static final ZEncoder[] swervemod_encoder = { + new TalonSRXEncoder(9).set_zero(0), + new TalonSRXEncoder(10).set_zero(0), + new TalonSRXEncoder(11).set_zero(0), + new TalonSRXEncoder(12).set_zero(0), + }; + + public static final FalconWithEncoder[] angle_motor = { + new FalconWithEncoder(5, swervemod_encoder[0]), + new FalconWithEncoder(6, swervemod_encoder[1]), + new FalconWithEncoder(7, swervemod_encoder[2]), + new FalconWithEncoder(8, swervemod_encoder[3]), + }; + private static final FalconSwerve[] mods = { - new FalconSwerve(1, 2), - new FalconSwerve(3, 4), - new FalconSwerve(5, 6), - new FalconSwerve(7, 8), + new FalconSwerve(speed_motor[0], angle_motor[0]), + new FalconSwerve(speed_motor[1], angle_motor[1]), + new FalconSwerve(speed_motor[2], angle_motor[2]), + new FalconSwerve(speed_motor[3], angle_motor[3]), }; private static final Pigeon gyro = new Pigeon(0); @@ -23,7 +49,7 @@ public class Chassis extends Zwerve { * Creates a new Chassis. */ public Chassis() { - super(mods, gyro, 114, 114); + super(mods, gyro, new Vec2D(114, 114)); // Mod I. mods[0].speed_motor.set_pid(0, 0, 0); mods[0].angle_motor.set_pid(0, 0, 0); @@ -45,6 +71,7 @@ public Chassis() { @Override public void periodic() { // This method will be called once per scheduler run + super.periodic(); } @Override