From f5a543758ca45d60011580f88a77378bb3a42d6f Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Sun, 22 Jan 2023 12:37:20 -0600 Subject: [PATCH] All motors now use velocity instead of power --- .../minibots/cyberarm/chiron/Robot.java | 3 +- .../states/teleop/ArmDriverControl.java | 6 ++-- .../teleop/DrivetrainDriverControl.java | 33 ++++++++++--------- 3 files changed, 23 insertions(+), 19 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java index 48219ec..0e4b98a 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java @@ -129,7 +129,8 @@ public class Robot { arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); // MOTOR POWER - arm.setPower(tuningConfig("arm_power").value()); + arm.setVelocity( + angleToTicks(tuningConfig("arm_velocity_in_degrees_per_second").value())); // SERVOS (POSITIONAL) // Gripper diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java index 02481e2..7c2ea54 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java @@ -3,6 +3,7 @@ package org.timecrafters.minibots.cyberarm.chiron.states.teleop; import com.qualcomm.robotcore.hardware.Gamepad; import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.timecrafters.minibots.cyberarm.chiron.Robot; public class ArmDriverControl extends CyberarmState { @@ -86,7 +87,7 @@ public class ArmDriverControl extends CyberarmState { } private void stopArm() { - robot.arm.setPower(0); + robot.arm.setVelocity(0); } private void automatics() { @@ -115,7 +116,8 @@ public class ArmDriverControl extends CyberarmState { stopArm(); } else { - robot.arm.setPower(robot.tuningConfig("arm_power").value()); + robot.arm.setVelocity( + robot.angleToTicks(robot.tuningConfig("arm_velocity_in_degrees_per_second").value())); } } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java index f5c6626..f14262e 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java @@ -3,6 +3,7 @@ package org.timecrafters.minibots.cyberarm.chiron.states.teleop; import com.qualcomm.robotcore.hardware.Gamepad; import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.timecrafters.minibots.cyberarm.chiron.Robot; public class DrivetrainDriverControl extends CyberarmState { @@ -41,20 +42,15 @@ public class DrivetrainDriverControl extends CyberarmState { engine.telemetry.addData("Robot Slow Mode", robotSlowMode); } - // FIXME: replace .setPower with .setVelocity // REF: https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html private void move() { if (robot.automaticAntiTipActive || robot.hardwareFault) { return; } - double maxSpeed = robot.tuningConfig("drivetrain_max_power").value(); - double slowSpeed = robot.tuningConfig("drivetrain_slow_power").value(); - double speedLimiter = robotSlowMode ? slowSpeed : maxSpeed; - - double y = (invertRobotForward ? controller.left_stick_y : -controller.left_stick_y) * speedLimiter; - double x = ((invertRobotForward ? controller.left_stick_x : -controller.left_stick_x) * speedLimiter) * 1.1; // Counteract imperfect strafing - double rx = -controller.right_stick_x * speedLimiter; + 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 rx = -controller.right_stick_x; double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); @@ -77,19 +73,24 @@ public class DrivetrainDriverControl extends CyberarmState { backRightPower = (y + x - rx) / denominator; } - robot.frontLeftDrive.setPower(frontLeftPower); - robot.frontRightDrive.setPower(frontRightPower); + double maxVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.tuningConfig("drivetrain_max_velocity_in_inches").value()); + double slowVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.tuningConfig("drivetrain_slow_velocity_in_inches").value()); + double velocity = robotSlowMode ? slowVelocity : maxVelocity; - robot.backLeftDrive.setPower(backLeftPower); - robot.backRightDrive.setPower(backRightPower); + // Power is treated as a ratio here + robot.frontLeftDrive.setVelocity(frontLeftPower * velocity); + robot.frontRightDrive.setVelocity(frontRightPower * velocity); + + robot.backLeftDrive.setVelocity(backLeftPower * velocity); + robot.backRightDrive.setVelocity(backRightPower * velocity); } private void stopDrive() { - robot.backLeftDrive.setPower(0); - robot.frontRightDrive.setPower(0); + robot.backLeftDrive.setVelocity(0); + robot.frontRightDrive.setVelocity(0); - robot.frontLeftDrive.setPower(0); - robot.backRightDrive.setPower(0); + robot.frontLeftDrive.setVelocity(0); + robot.backRightDrive.setVelocity(0); } private void automatics() {