TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-15 17:52: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 FieldLocalizer fieldLocalizer;
private final double radius, diameter; 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 boolean LEDStatusToggle = false;
private double lastLEDStatusAnimationTime = 0; private double lastLEDStatusAnimationTime = 0;
@@ -65,9 +66,6 @@ public class Robot {
this.configuration = configuration; this.configuration = configuration;
this.fieldLocalizer = fieldLocalizer; this.fieldLocalizer = fieldLocalizer;
this.fieldLocalizer.setRobot(this);
this.fieldLocalizer.standardSetup();
radius = tuningConfig("field_localizer_robot_radius").value(); radius = tuningConfig("field_localizer_robot_radius").value();
diameter = radius * 2; diameter = radius * 2;
@@ -153,6 +151,10 @@ public class Robot {
RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD)); RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD));
imu.initialize(parameters); imu.initialize(parameters);
// INITIALIZE AFTER EVERYTHING ELSE to prevent use before set crashes
this.fieldLocalizer.setRobot(this);
this.fieldLocalizer.standardSetup();
} }
public void standardTelemetry() { public void standardTelemetry() {
@@ -231,6 +233,18 @@ public class Robot {
engine.telemetry.addData(" Arm", arm.getTargetPosition()); 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(); engine.telemetry.addLine();
// Servos // Servos
@@ -318,9 +332,11 @@ 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 inches = unit.toInches(unit.fromUnit(unit, distance)); // NOTE: UNTESTED 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 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 fieldLocalizer
); );
robot.imu.resetYaw();
addTask(fieldLocalizer); addTask(fieldLocalizer);
setupFromConfig( 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 positionXInInches, positionYInInches, toleranceInInches, facing, targetVelocity, timeInMS;
private final double maxSpeed; private final double maxVelocity;
private double speed; private double speed;
private int distanceAlreadyTravelled; private int distanceAlreadyTravelled;
@@ -25,11 +25,11 @@ public class Move extends CyberarmState {
positionYInInches = robot.getConfiguration().variable(groupName, actionName, "positionYInInches").value(); positionYInInches = robot.getConfiguration().variable(groupName, actionName, "positionYInInches").value();
toleranceInInches = robot.getConfiguration().variable(groupName, actionName, "toleranceInInches").value(); toleranceInInches = robot.getConfiguration().variable(groupName, actionName, "toleranceInInches").value();
facing = robot.getConfiguration().variable(groupName, actionName, "facing").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(); timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value();
stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled; 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; speed = 0.0;
} }
@@ -72,11 +72,13 @@ public class Move extends CyberarmState {
} }
private void move() { private void move() {
if (Math.abs(speed) > maxSpeed) { if (Math.abs(speed) > maxVelocity) {
speed = speed < 0 ? -maxSpeed : maxSpeed; 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 rightSpeed = 0.0;
double rotateRightSpeed = 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 // FIXME: targetSpeed * speed[Limiter] will cause infinitesimal power issues
double y = -forwardSpeed * speed; double y = -forwardSpeed * speed;
double x = (-rightSpeed * speed) * 1.1; // Counteract imperfect strafing. TODO: make tunable 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; frontRightPower = (rotY - rotX + rx) / denominator;
backRightPower = (rotY + rotX - rx) / denominator; backRightPower = (rotY + rotX - rx) / denominator;
robot.frontLeftDrive.setPower(frontLeftPower); robot.frontLeftDrive.setVelocity(frontLeftPower * targetVelocity);
robot.frontRightDrive.setPower(frontRightPower); robot.frontRightDrive.setVelocity(frontRightPower * targetVelocity);
robot.backLeftDrive.setPower(backLeftPower); robot.backLeftDrive.setVelocity(backLeftPower * targetVelocity);
robot.backRightDrive.setPower(backRightPower); robot.backRightDrive.setVelocity(backRightPower * targetVelocity);
} }
@Override @Override
public void stop() { public void stop() {
robot.frontLeftDrive.setPower(0); robot.backLeftDrive.setVelocity(0); robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(0); robot.frontRightDrive.setVelocity(0); robot.frontRightDrive.setPower(0);
robot.backLeftDrive.setPower(0); robot.frontLeftDrive.setVelocity(0); robot.backLeftDrive.setPower(0);
robot.backRightDrive.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 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 rx = -controller.right_stick_x;
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); 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() { private void stopDrive() {
robot.backLeftDrive.setVelocity(0); robot.backLeftDrive.setVelocity(0); robot.backLeftDrive.setPower(0);
robot.frontRightDrive.setVelocity(0); robot.frontRightDrive.setVelocity(0); robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setVelocity(0); robot.frontLeftDrive.setVelocity(0); robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setVelocity(0); robot.backRightDrive.setVelocity(0); robot.backRightDrive.setPower(0);
} }
private void automatics() { private void automatics() {
@@ -130,7 +130,7 @@ public class DrivetrainDriverControl extends CyberarmState {
robot.hardwareFault = !robot.hardwareFault; robot.hardwareFault = !robot.hardwareFault;
} }
if (button.equals("pause")) { if (button.equals("back")) {
invertRobotForward = !invertRobotForward; 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; deltaX = deltaXRotation * cosineTheta - deltaYRotation * sinTheta;
deltaY = deltaYRotation * cosineTheta + deltaXRotation * sinTheta; deltaY = deltaYRotation * cosineTheta + deltaXRotation * sinTheta;
posX = lastX + deltaX; posX += lastX + deltaX;
posY = lastY + deltaY; posY += lastY + deltaY;
lastFrontLeft = robot.frontLeftDrive.getCurrentPosition(); lastFrontLeft = robot.frontLeftDrive.getCurrentPosition();
lastFrontRight = robot.frontRightDrive.getCurrentPosition(); lastFrontRight = robot.frontRightDrive.getCurrentPosition();