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

Implement Robot#unitToTicks and Robot#ticksToUnit- needs testing

Browse Source
This commit is contained in:
Cyberarm
2023-01-22 09:10:31 -06:00
parent 6fea7e69aa
commit afebf84273
1 changed files with 13 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -55,6 +55,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, ticksPerRevolution;
private boolean LEDStatusToggle = false; private boolean LEDStatusToggle = false;
private double lastLEDStatusAnimationTime = 0; private double lastLEDStatusAnimationTime = 0;
@@ -71,6 +73,11 @@ public class Robot {
imuAngleOffset = hardwareConfig("imu_angle_offset").value(); imuAngleOffset = hardwareConfig("imu_angle_offset").value();
wheelRadius = tuningConfig("wheel_radius").value();
gearRatio = tuningConfig("wheel_gear_ratio").value();
ticksPerRevolution = tuningConfig("wheel_ticks_per_revolution").value();
// FIXME: Rename motors in configuration // FIXME: Rename motors in configuration
// Define hardware // Define hardware
backLeftDrive = engine.hardwareMap.get(DcMotorEx.class, "left_drive"); // MOTOR PORT: ? backLeftDrive = engine.hardwareMap.get(DcMotorEx.class, "left_drive"); // MOTOR PORT: ?
@@ -312,17 +319,17 @@ public class Robot {
public int unitToTicks(DistanceUnit unit, double distance) { public int unitToTicks(DistanceUnit unit, double distance) {
double inches = unit.toInches(unit.fromUnit(unit, distance)); // NOTE: UNTESTED double inches = unit.toInches(unit.fromUnit(unit, distance)); // NOTE: UNTESTED
// FIXME: This should be stored as a presudo constant at initialization double ticks = wheelRadius * 2 * Math.PI * gearRatio * inches * ticksPerRevolution;
double wheelRadius = tuningConfig("wheel_radius").value();
double gearRatio = tuningConfig("wheel_gear_ratio").value();
double ticksPerRevolution = tuningConfig("wheel_ticks_per_revolution").value();
return 0; return (int)ticks; // NOTE: UNTESTED
} }
// For: Drive Wheels // For: Drive Wheels
public double ticksToUnit(DistanceUnit unit, int ticks) { public double ticksToUnit(DistanceUnit unit, int ticks) {
return 0; // Convert to inches, then to unit.
double inches = wheelRadius * 2 * Math.PI * gearRatio * ticks / ticksPerRevolution;
return unit.fromUnit(DistanceUnit.INCH, inches); // NOTE: UNTESTED
} }
// For: Arm // For: Arm