TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-15 20:12:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

Refactor Robot to use gear ratio multiplied by ticks per revolution of raw motor for Arm

Browse Source
This commit is contained in:
cyberarm
2023-01-24 22:53:51 -06:00
parent 77c103ecc2
commit 6f62886453
1 changed files with 10 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java
View File

@@ -71,8 +71,8 @@ public class Robot {
private final FieldLocalizer fieldLocalizer; private final FieldLocalizer fieldLocalizer;
private final double radius, diameter; private final double radius, diameter;
private final double wheelRadius, gearRatio; private final double wheelRadius, wheelGearRatio, armGearRatio;
private final int ticksPerRevolution; private final int wheelTicksPerRevolution, armTicksPerRevolution;
private boolean LEDStatusToggle = false; private boolean LEDStatusToggle = false;
private double lastLEDStatusAnimationTime = 0; private double lastLEDStatusAnimationTime = 0;
@@ -88,9 +88,11 @@ public class Robot {
imuAngleOffset = hardwareConfig("imu_angle_offset").value(); imuAngleOffset = hardwareConfig("imu_angle_offset").value();
wheelRadius = tuningConfig("wheel_radius").value(); wheelRadius = tuningConfig("wheel_radius").value();
gearRatio = tuningConfig("wheel_gear_ratio").value(); wheelGearRatio = tuningConfig("wheel_gear_ratio").value();
ticksPerRevolution = tuningConfig("wheel_ticks_per_revolution").value(); wheelTicksPerRevolution = tuningConfig("wheel_ticks_per_revolution").value();
armGearRatio = tuningConfig("arm_gear_ratio").value();
armTicksPerRevolution = tuningConfig("arm_ticks_per_revolution").value();
// FIXME: Rename motors in configuration // FIXME: Rename motors in configuration
// Define hardware // Define hardware
@@ -450,7 +452,7 @@ public class Robot {
// For: Drive Wheels // For: Drive Wheels
public int unitToTicks(DistanceUnit unit, double distance) { public int unitToTicks(DistanceUnit unit, double distance) {
double fI = (gearRatio * ticksPerRevolution) / (wheelRadius * 2 * Math.PI * (gearRatio * ticksPerRevolution) / (gearRatio * ticksPerRevolution)); double fI = (wheelGearRatio * wheelTicksPerRevolution) / (wheelRadius * 2 * Math.PI * (wheelGearRatio * wheelTicksPerRevolution) / (wheelGearRatio * wheelTicksPerRevolution));
double inches = unit.toInches(unit.fromUnit(unit, distance)); double inches = unit.toInches(unit.fromUnit(unit, distance));
@@ -462,16 +464,14 @@ public class Robot {
// For: Drive Wheels // For: Drive Wheels
public double ticksToUnit(DistanceUnit unit, int ticks) { public double ticksToUnit(DistanceUnit unit, int ticks) {
// Convert to inches, then to unit. // Convert to inches, then to unit.
double inches = wheelRadius * 2 * Math.PI * ticks / (gearRatio * ticksPerRevolution); double inches = wheelRadius * 2 * Math.PI * ticks / (wheelGearRatio * wheelTicksPerRevolution);
return unit.fromUnit(DistanceUnit.INCH, inches); return unit.fromUnit(DistanceUnit.INCH, inches);
} }
// For: Arm // For: Arm
public int angleToTicks(double angle) { public int angleToTicks(double angle) {
int ticksPerRevolution = tuningConfig("arm_ticks_per_revolution").value(); double d = (armGearRatio * armTicksPerRevolution) / 360.0;
double d = ticksPerRevolution / 360.0;
// Casting to float so that the int version of Math.round is used. // Casting to float so that the int version of Math.round is used.
return Math.round((float)d * (float)angle); return Math.round((float)d * (float)angle);
@@ -479,9 +479,7 @@ public class Robot {
// For: Arm // For: Arm
public double ticksToAngle(int ticks) { public double ticksToAngle(int ticks) {
int ticksPerRevolution = tuningConfig("arm_ticks_per_revolution").value(); double oneDegree = 360.0 / (armGearRatio * armTicksPerRevolution);
double oneDegree = 360.0 / ticksPerRevolution;
return oneDegree * ticks; return oneDegree * ticks;
} }