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

Tweaks to CHIRON

Browse Source
This commit is contained in:
Cyberarm
2023-01-22 17:35:30 -06:00
parent f5a543758c
commit 3286b1ec57
5 changed files with 49 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -55,7 +55,8 @@ public class Robot {
private final FieldLocalizer fieldLocalizer;
private final double radius, diameter;
private final double wheelRadius, gearRatio, ticksPerRevolution;
private final double wheelRadius, gearRatio;
private final int ticksPerRevolution;
private boolean LEDStatusToggle = false;
private double lastLEDStatusAnimationTime = 0;
@@ -65,9 +66,6 @@ public class Robot {
this.configuration = configuration;
this.fieldLocalizer = fieldLocalizer;
this.fieldLocalizer.setRobot(this);
this.fieldLocalizer.standardSetup();
radius = tuningConfig("field_localizer_robot_radius").value();
diameter = radius * 2;
@@ -153,6 +151,10 @@ public class Robot {
RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD));
imu.initialize(parameters);
// INITIALIZE AFTER EVERYTHING ELSE to prevent use before set crashes
this.fieldLocalizer.setRobot(this);
this.fieldLocalizer.standardSetup();
}
public void standardTelemetry() {
@@ -231,6 +233,18 @@ public class Robot {
engine.telemetry.addData(" Arm", arm.getTargetPosition());
// Motor Velocity
engine.telemetry.addLine("Motor Velocity");
engine.telemetry.addData(" Front Left Drive", frontLeftDrive.getVelocity());
engine.telemetry.addData(" Front Right Drive", frontRightDrive.getVelocity());
engine.telemetry.addData(" Back Left Drive", backLeftDrive.getVelocity());
engine.telemetry.addData(" Back Right Drive", backRightDrive.getVelocity());
engine.telemetry.addLine();
engine.telemetry.addData(" Arm", arm.getVelocity());
engine.telemetry.addLine();
// Servos
@@ -318,9 +332,11 @@ public class Robot {
// For: Drive Wheels
public int unitToTicks(DistanceUnit unit, double distance) {
double fI = (gearRatio * ticksPerRevolution) / (wheelRadius * 2 * Math.PI * (gearRatio * ticksPerRevolution) / (gearRatio * ticksPerRevolution));
double inches = unit.toInches(unit.fromUnit(unit, distance)); // NOTE: UNTESTED
double ticks = (wheelRadius * 2 * Math.PI) / inches * (gearRatio * ticksPerRevolution);
double ticks = fI * inches;
return (int)ticks; // NOTE: UNTESTED
}

2
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousRedLeftSideEngine.java
View File

@@ -28,6 +28,8 @@ public class AutonomousRedLeftSideEngine extends CyberarmEngine {
fieldLocalizer
);
robot.imu.resetYaw();
addTask(fieldLocalizer);
setupFromConfig(

32
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java
View File

@@ -10,7 +10,7 @@ public class Move extends CyberarmState {
private final double positionXInInches, positionYInInches, toleranceInInches, facing, targetVelocity, timeInMS;
private final double maxSpeed;
private final double maxVelocity;
private double speed;
private int distanceAlreadyTravelled;
@@ -25,11 +25,11 @@ public class Move extends CyberarmState {
positionYInInches = robot.getConfiguration().variable(groupName, actionName, "positionYInInches").value();
toleranceInInches = robot.getConfiguration().variable(groupName, actionName, "toleranceInInches").value();
facing = robot.getConfiguration().variable(groupName, actionName, "facing").value();
targetVelocity = robot.getConfiguration().variable(groupName, actionName, "targetVelocity").value();
targetVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "targetVelocity").value());
timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value();
stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled;
maxSpeed = robot.tuningConfig("drivetrain_max_power").value();
maxVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.tuningConfig("drivetrain_max_velocity_in_inches").value());
speed = 0.0;
}
@@ -72,11 +72,13 @@ public class Move extends CyberarmState {
}
private void move() {
if (Math.abs(speed) > maxSpeed) {
speed = speed < 0 ? -maxSpeed : maxSpeed;
if (Math.abs(speed) > maxVelocity) {
speed = speed < 0 ? -maxVelocity : maxVelocity;
}
double forwardSpeed = 1.0; //distanceInInches < 0 ? -1.0 : 1.0;
speed = 1.0;
double forwardSpeed = -1.0; //distanceInInches < 0 ? -1.0 : 1.0;
double rightSpeed = 0.0;
double rotateRightSpeed = 0.0;
@@ -88,6 +90,8 @@ public class Move extends CyberarmState {
}
}
rotateRightSpeed = 0;
// FIXME: targetSpeed * speed[Limiter] will cause infinitesimal power issues
double y = -forwardSpeed * speed;
double x = (-rightSpeed * speed) * 1.1; // Counteract imperfect strafing. TODO: make tunable
@@ -106,19 +110,19 @@ public class Move extends CyberarmState {
frontRightPower = (rotY - rotX + rx) / denominator;
backRightPower = (rotY + rotX - rx) / denominator;
robot.frontLeftDrive.setPower(frontLeftPower);
robot.frontRightDrive.setPower(frontRightPower);
robot.frontLeftDrive.setVelocity(frontLeftPower * targetVelocity);
robot.frontRightDrive.setVelocity(frontRightPower * targetVelocity);
robot.backLeftDrive.setPower(backLeftPower);
robot.backRightDrive.setPower(backRightPower);
robot.backLeftDrive.setVelocity(backLeftPower * targetVelocity);
robot.backRightDrive.setVelocity(backRightPower * targetVelocity);
}
@Override
public void stop() {
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(0);
robot.backLeftDrive.setVelocity(0); robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setVelocity(0); robot.frontRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.frontLeftDrive.setVelocity(0); robot.backLeftDrive.setPower(0);
robot.backRightDrive.setVelocity(0); robot.backRightDrive.setPower(0);
}
}

12
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java
View File

@@ -49,7 +49,7 @@ public class DrivetrainDriverControl extends CyberarmState {
}
double y = invertRobotForward ? controller.left_stick_y : -controller.left_stick_y;
double x = (invertRobotForward ? controller.left_stick_x : -controller.left_stick_x) * 1.1; // Counteract imperfect strafing
double x = (invertRobotForward && !fieldCentricControl ? controller.left_stick_x : -controller.left_stick_x) * 1.1; // Counteract imperfect strafing
double rx = -controller.right_stick_x;
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
@@ -86,11 +86,11 @@ public class DrivetrainDriverControl extends CyberarmState {
}
private void stopDrive() {
robot.backLeftDrive.setVelocity(0);
robot.frontRightDrive.setVelocity(0);
robot.backLeftDrive.setVelocity(0); robot.backLeftDrive.setPower(0);
robot.frontRightDrive.setVelocity(0); robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setVelocity(0);
robot.backRightDrive.setVelocity(0);
robot.frontLeftDrive.setVelocity(0); robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setVelocity(0); robot.backRightDrive.setPower(0);
}
private void automatics() {
@@ -130,7 +130,7 @@ public class DrivetrainDriverControl extends CyberarmState {
robot.hardwareFault = !robot.hardwareFault;
}
if (button.equals("pause")) {
if (button.equals("back")) {
invertRobotForward = !invertRobotForward;
}

4
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/FieldLocalizer.java
View File

@@ -132,8 +132,8 @@ public class FieldLocalizer extends CyberarmState {
deltaX = deltaXRotation * cosineTheta - deltaYRotation * sinTheta;
deltaY = deltaYRotation * cosineTheta + deltaXRotation * sinTheta;
posX = lastX + deltaX;
posY = lastY + deltaY;
posX += lastX + deltaX;
posY += lastY + deltaY;
lastFrontLeft = robot.frontLeftDrive.getCurrentPosition();
lastFrontRight = robot.frontRightDrive.getCurrentPosition();