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

Sync changes

Browse Source
This commit is contained in:
Cyberarm
2023-01-21 12:20:22 -06:00
parent 2760e97507
commit 740a6e97e0
4 changed files with 45 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,11 +1,14 @@
package org.cyberarm.engine.V2;
import android.util.Log;
import com.qualcomm.robotcore.hardware.Gamepad;
import java.lang.reflect.Field;
import java.util.HashMap;
public class GamepadChecker {
private final String TAG = "GamepadChecker";
private final CyberarmEngine engine;
private final Gamepad gamepad;
private final HashMap<String, Boolean> buttons = new HashMap<>();
@@ -49,16 +52,23 @@ public class GamepadChecker {
if (button) {
if (!buttons.get(btn)) {
engine.buttonDown(gamepad, btn);
Log.d(TAG, "Button '" + btn + "' is DOWN [" + buttons.size() + "]");
}
buttons.put(btn, true);
} else {
if (buttons.get(btn)) {
engine.buttonUp(gamepad, btn);
Log.d(TAG, "Button '" + btn + "' is UP");
}
buttons.put(btn, false);
}
buttonsDebounceInterval.put(btn, milliseconds);
} else if (button == buttons.get(btn)) {
buttonsDebounceInterval.put(btn, milliseconds);
}

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

@@ -222,7 +222,8 @@ public class Robot {
// Sensors / IMU
engine.telemetry.addLine("IMU");
engine.telemetry.addData(" Facing", facing());
engine.telemetry.addData(" Facing (Degrees)", facing());
engine.telemetry.addData(" Heading (Radians)", heading());
engine.telemetry.addData(" Turn Rate", turnRate());
}

10
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/ConfigureAndTestHardwareEngine.java
View File

@@ -26,13 +26,13 @@ public class ConfigureAndTestHardwareEngine extends CyberarmEngine {
public void setup() {
robot = new Robot(this, new TimeCraftersConfiguration("CHIRON"));
addState(new MotorSetupState("Left Drive", robot.backLeftDrive, "left_drive_direction_forward", robot));
addState(new MotorSetupState("Right Drive", robot.frontRightDrive, "right_drive_direction_forward", robot));
addState(new MotorSetupState("Back Left Drive", robot.backLeftDrive, "back_left_drive_direction_forward", robot));
addState(new MotorSetupState("Front Right Drive", robot.frontRightDrive, "front_right_drive_direction_forward", robot));
addState(new MotorSetupState("Front Drive", robot.frontLeftDrive, "front_drive_direction_forward", robot));
addState(new MotorSetupState("Back Drive", robot.backRightDrive, "back_drive_direction_forward", robot));
addState(new MotorSetupState("Front Left Drive", robot.frontLeftDrive, "front_left_drive_direction_forward", robot));
addState(new MotorSetupState("Back Right Drive", robot.backRightDrive, "back_right_drive_direction_forward", robot));
addState(new MotorSetupState("Lift Drive", robot.arm, "lift_drive_direction_forward", robot));
addState(new MotorSetupState("Arm", robot.arm, "arm_direction_forward", robot));
}
@Override

42
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/DriverControlState.java
View File

@@ -1,5 +1,7 @@
package org.timecrafters.minibots.cyberarm.chiron.states;
import android.util.Log;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.cyberarm.engine.V2.CyberarmState;
@@ -13,6 +15,8 @@ public class DriverControlState extends CyberarmState {
private double lastArmManualControlTime = 0, lastWristManualControlTime = 0, lastLEDStatusAnimationTime = 0;
private boolean LEDStatusToggle = false;
private boolean fieldCentricControl = true;
private boolean invertRobotForward = false;
private boolean robotSlowMode = false;
public DriverControlState(Robot robot) {
this.robot = robot;
@@ -41,6 +45,7 @@ public class DriverControlState extends CyberarmState {
engine.telemetry.addData("Wrist Interval", lastWristManualControlTime);
engine.telemetry.addData("LED Status Interval", lastLEDStatusAnimationTime);
engine.telemetry.addData("Field Centric Control", fieldCentricControl);
engine.telemetry.addData("Invert Robot Forward", invertRobotForward);
}
// FIXME: replace .setPower with .setVelocity
@@ -50,27 +55,32 @@ public class DriverControlState extends CyberarmState {
return;
}
double y = -engine.gamepad1.left_stick_y;
double x = engine.gamepad1.left_stick_x * 1.1;
double rx = engine.gamepad1.right_stick_x;
double maxSpeed = robot.tuningConfig("drivetrain_max_power").value();
double slowSpeed = robot.tuningConfig("drivetrain_slow_power").value();
double speedLimiter = robotSlowMode ? slowSpeed : maxSpeed;
double y = (invertRobotForward ? engine.gamepad1.left_stick_y : -engine.gamepad1.left_stick_y) * speedLimiter;
double x = ((invertRobotForward ? engine.gamepad1.left_stick_x : -engine.gamepad1.left_stick_x) * speedLimiter) * 1.1; // Counteract imperfect strafing
double rx = -engine.gamepad1.right_stick_x * speedLimiter;
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double frontLeftPower = 0, frontRightPower = 0, backLeftPower = 0 , backRightPower = 0;
if (fieldCentricControl) {
double heading = -robot.heading();
double heading = robot.heading();
double rotX = x * Math.cos(heading) - y * Math.sin(heading);
double rotY = x * Math.sin(heading) + y * Math.cos(heading);
frontLeftPower = (rotY + rotX + rx) / denominator;
backLeftPower = (rotY - rotX + rx) / denominator;
frontRightPower = (rotY - rotX - rx) / denominator;
backLeftPower = (rotY - rotX - rx) / denominator;
frontRightPower = (rotY - rotX + rx) / denominator;
backRightPower = (rotY + rotX - rx) / denominator;
} else {
frontLeftPower = (y + x + rx) / denominator;
backLeftPower = (y - x + rx) / denominator;
frontRightPower = (y - x - rx) / denominator;
backLeftPower = (y - x - rx) / denominator;
frontRightPower = (y - x + rx) / denominator;
backRightPower = (y + x - rx) / denominator;
}
@@ -289,6 +299,16 @@ public class DriverControlState extends CyberarmState {
} else if (button.equals("y")) {
armPosition(Robot.ArmPosition.MEDIUM);
}
if (button.equals("pause")) {
invertRobotForward = !invertRobotForward;
}
if (button.equals("guide")) {
robot.hardwareFault = !robot.hardwareFault;
}
if (button.equals("start")) {
fieldCentricControl = !fieldCentricControl;
}
}
@Override
@@ -296,11 +316,5 @@ public class DriverControlState extends CyberarmState {
if (gamepad != engine.gamepad1) {
return;
}
if (button.equals("guide")) {
robot.hardwareFault = !robot.hardwareFault;
} else if (button.equals("start")) {
fieldCentricControl = !fieldCentricControl;
}
}
}