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

Disable time de-bouncing from GamepadChecker (This already appears to be done for us), misc changes for CHIRON

Browse Source
This commit is contained in:
Cyberarm
2023-01-24 21:03:50 -06:00
parent 8bed3ffefd
commit 77c103ecc2
6 changed files with 91 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java
View File

@@ -13,7 +13,7 @@ public class GamepadChecker {
private final Gamepad gamepad;
private final HashMap<String, Boolean> buttons = new HashMap<>();
private final HashMap<String, Long> buttonsDebounceInterval = new HashMap<>();
private final long debounceTime = 20L; // ms
private final long debounceTime = 0L; // ms
public GamepadChecker(CyberarmEngine engine, Gamepad gamepad) {
this.engine = engine;

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

@@ -7,6 +7,7 @@ import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.Blinker;
import com.qualcomm.robotcore.hardware.CRServoImplEx;
import com.qualcomm.robotcore.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
@@ -32,7 +33,8 @@ import java.util.concurrent.TimeUnit;
public class Robot {
private static final String TAG = "CHIRON | Robot";
public final DcMotorEx backLeftDrive, frontRightDrive, frontLeftDrive, backRightDrive, arm;
public final ServoImplEx gripper, wrist;
public final ServoImplEx gripper;
public final CRServoImplEx wrist;
public final IMU imu;
public final ColorSensor indicatorA, indicatorB;
public LynxModule expansionHub;
@@ -102,7 +104,7 @@ public class Robot {
arm = engine.hardwareMap.get(DcMotorEx.class, "lift_drive"); // MOTOR PORT: ?
gripper = engine.hardwareMap.get(ServoImplEx.class, "gripper"); // SERVO PORT: ?
wrist = engine.hardwareMap.get(ServoImplEx.class, "wrist"); // SERVO PORT: ?
wrist = engine.hardwareMap.get(CRServoImplEx.class, "wrist"); // SERVO PORT: ?
indicatorA = engine.hardwareMap.colorSensor.get("indicator_A"); // I2C
indicatorB = engine.hardwareMap.colorSensor.get("indicator_B"); // I2C
@@ -141,17 +143,19 @@ public class Robot {
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// MOTOR POWER
arm.setVelocity(
angleToTicks(tuningConfig("arm_velocity_in_degrees_per_second").value()));
// arm.setVelocity(
// angleToTicks(tuningConfig("arm_velocity_in_degrees_per_second").value()));
arm.setPower(0.75);
// SERVOS (POSITIONAL)
// Gripper
gripper.setDirection(hardwareConfig("gripper_direction_forward").value() ? Servo.Direction.FORWARD : Servo.Direction.REVERSE);
gripper.setPosition(tuningConfig("gripper_initial_position").value());
// SERVOS (CONTINUOUS)
// Wrist
wrist.setDirection(hardwareConfig("wrist_direction_forward").value() ? Servo.Direction.FORWARD : Servo.Direction.REVERSE);
wrist.setPosition(tuningConfig("wrist_initial_position").value());
wrist.setDirection(hardwareConfig("wrist_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
wrist.setPower(tuningConfig("wrist_up_power").value());
// SENSORS
// COLOR SENSORS
@@ -232,7 +236,7 @@ public class Robot {
engine.telemetry.addLine();
engine.telemetry.addData(" Arm", "%d (%8.2f in)", arm.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, arm.getTargetPosition()));
engine.telemetry.addData(" Arm", "%d (%8.2f degrees)", arm.getTargetPosition(), ticksToAngle(arm.getTargetPosition()));
// Motor Velocity
engine.telemetry.addLine("Motor Velocity");
@@ -283,7 +287,7 @@ public class Robot {
engine.telemetry.addData(" Gripper Enabled", gripper.isPwmEnabled());
engine.telemetry.addLine();
engine.telemetry.addData(" Wrist Direction", wrist.getDirection());
engine.telemetry.addData(" Wrist Position", wrist.getPosition());
engine.telemetry.addData(" Wrist Power", wrist.getPower());
engine.telemetry.addData(" Wrist Enabled", wrist.isPwmEnabled());
engine.telemetry.addLine();
@@ -293,6 +297,7 @@ public class Robot {
engine.telemetry.addData(" Facing (Degrees)", facing());
engine.telemetry.addData(" Heading (Radians)", heading());
engine.telemetry.addData(" Turn Rate", turnRate());
engine.telemetry.addData(" Angle Offset (Degrees)", imuAngleOffset);
engine.telemetry.addLine();
@@ -566,7 +571,10 @@ public class Robot {
double lastTiming = motorVelocityLastTiming.getOrDefault("Arm", time);
double deltaTime = (time - lastTiming) * 0.001;
double error = targetVelocity - arm.getVelocity();
double distanceToTarget = arm.getTargetPosition() - arm.getCurrentPosition();
double adjustedTargetVelocity = Math.abs(distanceToTarget) < targetVelocity ? Math.abs(distanceToTarget) : targetVelocity;
double error = adjustedTargetVelocity - arm.getVelocity();
double kp = 0.9;
newTargetVelocity += error * kp * deltaTime;
@@ -574,7 +582,9 @@ public class Robot {
motorTargetVelocity.put("Arm", newTargetVelocity);
motorVelocityLastTiming.put("Arm", time);
arm.setVelocity(newTargetVelocity);
// arm.setVelocity(newTargetVelocity);
arm.setPower(0.75);
}
public double getVoltage() {
@@ -584,11 +594,12 @@ public class Robot {
public double facing() {
double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
return (imuDegrees + imuAngleOffset + 360.0) % 360.0;
return (((imuDegrees + 360.0) % 360.0) + imuAngleOffset) % 360.0;
}
public double heading() {
return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
return AngleUnit.normalizeRadians(-facing() * Math.PI / 180.0);
// return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
}
public double turnRate() {

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

@@ -8,7 +8,7 @@ public class Move extends CyberarmState {
private final Robot robot;
private final String groupName, actionName;
private final double positionXInInches, positionYInInches, toleranceInInches, facing, targetVelocity, timeInMS;
private final double targetDistance, tolerance, facing, targetVelocity, timeInMS;
private final double maxVelocity;
private double speed;
@@ -21,11 +21,10 @@ public class Move extends CyberarmState {
this.groupName = groupName;
this.actionName = actionName;
positionXInInches = robot.getConfiguration().variable(groupName, actionName, "positionXInInches").value();
positionYInInches = robot.getConfiguration().variable(groupName, actionName, "positionYInInches").value();
toleranceInInches = robot.getConfiguration().variable(groupName, actionName, "toleranceInInches").value();
targetDistance = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "targetDistanceInInches").value());
tolerance = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "toleranceInInches").value());
facing = robot.getConfiguration().variable(groupName, actionName, "facing").value();
targetVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "targetVelocity").value());
targetVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "targetVelocityInInches").value());
timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value();
stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled;
@@ -36,7 +35,7 @@ public class Move extends CyberarmState {
@Override
public void start() {
// TODO: Use a dead wheel for this
distanceAlreadyTravelled = robot.frontLeftDrive.getCurrentPosition();
distanceAlreadyTravelled = robot.frontRightDrive.getCurrentPosition();
}
@Override
@@ -55,23 +54,33 @@ public class Move extends CyberarmState {
return;
}
// TODO: Double check maths
// int travelledDistance = (robot.frontLeftDrive.getCurrentPosition() - distanceAlreadyTravelled);
// int targetDistance = robot.unitToTicks(DistanceUnit.INCH, distanceInInches);
// int tolerance = robot.unitToTicks(DistanceUnit.INCH, toleranceInInches);
//
// if (robot.isBetween(targetDistance, travelledDistance - tolerance, travelledDistance + tolerance)) {
// stop();
//
// setHasFinished(true);
//
// return;
// }
int travelledDistance = -robot.frontRightDrive.getCurrentPosition() - distanceAlreadyTravelled;
move();
if (robot.isBetween(travelledDistance, targetDistance - tolerance, targetDistance + tolerance)) {
stop();
setHasFinished(true);
return;
}
moveDirectional(travelledDistance);
}
private void move() {
private void moveDirectional(double travelledDistance) {
// TODO: Ease-in-out velocity
double velocity = targetVelocity;
if (targetDistance > travelledDistance) {
robot.frontRightDrive.setVelocity(-velocity);
robot.backLeftDrive.setVelocity(-velocity);
} else {
robot.frontRightDrive.setVelocity(velocity);
robot.backLeftDrive.setVelocity(velocity);
}
}
private void moveOmni() {
if (Math.abs(speed) > maxVelocity) {
speed = speed < 0 ? -maxVelocity : maxVelocity;
}

32
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java
View File

@@ -78,18 +78,22 @@ public class ArmDriverControl extends CyberarmState {
return;
}
double stepInterval = robot.tuningConfig("wrist_manual_step_interval").value();
double stepSize = robot.tuningConfig("wrist_manual_step_size").value();
double stepPower = robot.tuningConfig("wrist_manual_step_power").value();
if ((controller.dpad_left || controller.dpad_right) && runTime() - lastArmManualControlTime >= stepInterval) {
lastArmManualControlTime = runTime();
if ((controller.dpad_left || controller.dpad_right)) {
robot.wristManuallyControlled = true;
if (controller.dpad_left) { // Wrist Left
robot.wrist.setPosition(robot.wrist.getPosition() - stepSize);
robot.wrist.setPower(stepPower);
} else if (controller.dpad_right) { // Wrist Right
robot.wrist.setPosition(robot.wrist.getPosition() + stepSize);
}
if (controller.dpad_right) { // Wrist Right
robot.wrist.setPower(-stepPower);
}
}
if (!controller.dpad_left && !controller.dpad_right) {
robot.wrist.setPower(0);
}
}
@@ -103,7 +107,7 @@ public class ArmDriverControl extends CyberarmState {
private void automatics() {
if (!robot.hardwareFault) {
automaticWrist();
// automaticWrist();
automaticArmVelocity();
}
@@ -118,9 +122,9 @@ public class ArmDriverControl extends CyberarmState {
double angle = robot.tuningConfig("wrist_auto_rotate_angle").value();
if (robot.ticksToAngle(robot.arm.getCurrentPosition()) >= angle) {
robot.wrist.setPosition(robot.tuningConfig("wrist_deposit_position").value());
robot.wrist.setPower(robot.tuningConfig("wrist_up_power").value());
} else {
robot.wrist.setPosition(robot.tuningConfig("wrist_collect_position").value());
robot.wrist.setPower(robot.tuningConfig("wrist_down_power").value());
}
}
@@ -209,11 +213,13 @@ public class ArmDriverControl extends CyberarmState {
if (button.equals("dpad_down")) {
robot.wristManuallyControlled = false;
robot.wrist.setPosition(robot.tuningConfig("wrist_deposit_position").value());
} else if (button.equals("dpad_up")) {
robot.wrist.setPower(robot.tuningConfig("wrist_up_power").value());
}
if (button.equals("dpad_up")) {
robot.wristManuallyControlled = false;
robot.wrist.setPosition(robot.tuningConfig("wrist_collect_position").value());
robot.wrist.setPower(robot.tuningConfig("wrist_down_power").value());
}
// Automatic Arm Control

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

@@ -1,5 +1,6 @@
package org.timecrafters.minibots.cyberarm.chiron.states.teleop;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.cyberarm.engine.V2.CyberarmState;
@@ -48,6 +49,7 @@ public class DrivetrainDriverControl extends CyberarmState {
double y = invertRobotForward ? controller.left_stick_y : -controller.left_stick_y;
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);