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

All motors now use velocity instead of power

Browse Source
This commit is contained in:
Cyberarm
2023-01-22 12:37:20 -06:00
parent 02640aedad
commit f5a543758c
3 changed files with 23 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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

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

@@ -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()));
}
}

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

@@ -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() {