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

Added theoretical servo controller that provides a position estimate, added drivetrain control for MentorBot Phoenix implementation, misc. tweaks.

Browse Source
This commit is contained in:
cyberarm
2023-03-18 15:08:41 -05:00
parent 2d1f930593
commit c5484131bb
4 changed files with 320 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

79
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/PositionalServoController.java Normal file
View File

@@ -0,0 +1,79 @@
package org.timecrafters.minibots.cyberarm.phoenix;
import com.qualcomm.robotcore.hardware.Servo;
public class PositionalServoController {
final private Servo servo;
final private double targetDegreesPerSecond, servoDegreesPerSecond, servoRangeInDegrees;
private double lastEstimatedPosition, estimatedPosition, workingPosition, targetPosition;
private long lastUpdatedAt;
private boolean travelling = false;
public PositionalServoController(Servo servo, double targetDegreesPerSecond, double servoDegreesPerSecond, double servoRangeInDegrees) {
this.servo = servo;
this.targetDegreesPerSecond = targetDegreesPerSecond;
this.servoDegreesPerSecond = servoDegreesPerSecond;
this.servoRangeInDegrees = servoRangeInDegrees;
this.workingPosition = servo.getPosition();
this.estimatedPosition = workingPosition;
this.lastEstimatedPosition = estimatedPosition;
this.lastUpdatedAt = System.currentTimeMillis();
}
public Servo getServo() {
return servo;
}
public void update() {
double error = targetPosition - estimatedPosition;
double delta = (System.currentTimeMillis() - lastUpdatedAt) / 1000.0;
double estimatedTravel = servoDegreesPerSecond * delta;
double targetTravel = targetDegreesPerSecond * delta;
if (targetTravel < estimatedTravel) {
estimatedTravel = targetTravel;
}
if (travelling) {
this.lastEstimatedPosition = estimatedPosition;
if (error < 0.0) {
this.estimatedPosition -= estimatedTravel;
} else {
this.estimatedPosition += estimatedTravel;
}
}
if (error < 0.0 - estimatedTravel) {
workingPosition -= estimatedTravel;
travelling = true;
} else if (error > 0.0 + estimatedTravel) {
workingPosition += estimatedTravel;
travelling = true;
} else {
travelling = false;
}
servo.setPosition(workingPosition);
this.lastUpdatedAt = System.currentTimeMillis();
}
public void setTargetPosition(double targetPosition) {
this.targetPosition = targetPosition;
}
public double getEstimatedPosition() {
return estimatedPosition;
}
public double getEstimatedAngle() {
return estimatedPosition * servoRangeInDegrees;
}
private double lerp(double min, double max, double t)
{
return min + (max - min) * t;
}
}

90
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/Robot.java
View File

@@ -3,18 +3,15 @@ package org.timecrafters.minibots.cyberarm.phoenix;
import android.annotation.SuppressLint;
import android.util.Log;
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.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.ServoImplEx;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.firstinspires.ftc.robotcore.external.ClassFactory;
@@ -27,7 +24,6 @@ import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
import org.timecrafters.minibots.cyberarm.chiron.tasks.FieldLocalizer;
import java.util.ArrayList;
import java.util.concurrent.ConcurrentHashMap;
@@ -37,6 +33,9 @@ import java.util.concurrent.TimeUnit;
public class Robot {
private static final String TAG = "Phoenix | Robot";
public final DcMotorEx backLeftDrive, frontRightDrive, frontLeftDrive, backRightDrive, arm;
public final PositionalServoController leftRiserServoController, rightRiserServoController;
public final Servo cameraServo;
public final CRServo collectorLeftServo, collectorRightServo;
// public final ServoImplEx gripper;
public final IMU imu;
public LynxModule expansionHub;
@@ -52,6 +51,7 @@ public class Robot {
private final ConcurrentHashMap<String, Double> motorVelocityError = new ConcurrentHashMap<>();
private final ConcurrentHashMap<String, Double> motorVelocityLastTiming = new ConcurrentHashMap<>();
private final ConcurrentHashMap<String, Double> motorTargetVelocity = new ConcurrentHashMap<>();
private ArmPosition armTargetPosition;
public enum ArmPosition {
COLLECT,
@@ -147,6 +147,33 @@ public class Robot {
// MOTOR POWER
arm.setPower(tuningConfig("arm_automatic_power").value());
// SERVOS
// NOTES: 428.57142858° per second = no load speed of 60° per 0.14s at 6V, see: https://docs.revrobotics.com/duo-build/actuators/servos/smart-robot-servo#mechanical-specifications
leftRiserServoController = new PositionalServoController(
engine.hardwareMap.servo.get("LowRiserLeft"),
5.0,
428.57142858,
270.0
); // SERVER PORT: ? - ? HUB
rightRiserServoController = new PositionalServoController(
engine.hardwareMap.servo.get("LowRiserRight"),
5.0,
428.57142858,
270.0
); // SERVER PORT: ? - ? HUB
cameraServo = engine.hardwareMap.servo.get("Camera Servo"); // SERVER PORT: ? - ? HUB
collectorLeftServo = engine.hardwareMap.crservo.get("Collector Left"); // SERVER PORT: ? - ? HUB
collectorRightServo = engine.hardwareMap.crservo.get("Collector Right"); // SERVER PORT: ? - ? HUB
// SERVO DIRECTIONS
cameraServo.setDirection(Servo.Direction.REVERSE);
collectorLeftServo.setDirection(DcMotorSimple.Direction.REVERSE);
collectorRightServo.setDirection(DcMotorSimple.Direction.FORWARD);
// SENSORS
// IMU
IMU.Parameters parameters = new IMU.Parameters(
new RevHubOrientationOnRobot(
@@ -394,6 +421,8 @@ public class Robot {
if (lastStatus == Status.DANGER) { expansionHub.setPattern(ledPatternDanger()); }
}
}
manageArmAndRiserServos();
}
public void stop() {
@@ -407,32 +436,67 @@ public class Robot {
return;
}
armTargetPosition = position;
reportStatus(Status.WARNING);
}
switch (position) {
private void manageArmAndRiserServos() {
boolean lowerServos = true;
switch (armTargetPosition) {
case COLLECT:
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_collect").value()));
if (areRiserServosInLoweredPosition()) {
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_collect").value()));
}
break;
case GROUND:
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_ground").value()));
break;
// case GROUND:
// if (areRiserServosInLoweredPosition()) {
// arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_ground").value()));
// }
// break;
case GROUND: // DISABLE GROUND JUNCTION SUPPORT FOR NOW
case LOW:
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_low").value()));
if (areRiserServosInLoweredPosition()) {
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_low").value()));
}
break;
case MEDIUM:
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_medium").value()));
if (areRiserServosInLoweredPosition()) {
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_medium").value()));
}
break;
case HIGH:
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_high").value()));
double angleHigh = tuningConfig("arm_position_angle_high").value();
arm.setTargetPosition(angleToTicks(angleHigh));
if (arm.getCurrentPosition() >= angleToTicks(angleHigh - 10.0)) {
lowerServos = false;
}
break;
default:
throw new RuntimeException("Unexpected arm position!");
}
if (lowerServos) {
leftRiserServoController.setTargetPosition(0.45);
rightRiserServoController.setTargetPosition(0.45);
} else {
leftRiserServoController.setTargetPosition(0.8);
rightRiserServoController.setTargetPosition(0.8);
}
leftRiserServoController.update();
rightRiserServoController.update();
}
private boolean areRiserServosInLoweredPosition() {
return leftRiserServoController.getEstimatedPosition() < 0.5 && rightRiserServoController.getEstimatedPosition() < 0.5;
}
// Adapted from: https://github.com/gosu/gosu/blob/980d64de2ce52e4b16fdd5cb9c9e11c8bbb80671/src/Math.cpp#L38

51
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/states/teleop/ArmController.java
View File

@@ -11,6 +11,7 @@ import org.timecrafters.minibots.cyberarm.phoenix.Robot;
public class ArmController extends CyberarmState {
private final Robot robot;
private final String groupName, actionName;
private final Gamepad controller;
private PIDController pidController;
private double p = 0, i = 0, d = 0, f = 0;
private final double ticksInDegree = 700 / 180;
@@ -20,6 +21,8 @@ public class ArmController extends CyberarmState {
this.groupName = groupName;
this.actionName = actionName;
this.controller = engine.gamepad1;
pidController = new PIDController(p, i, d);
}
@@ -38,7 +41,7 @@ public class ArmController extends CyberarmState {
case "derivative":
d = v.value();
break;
case "feed":
case "feed": // feedback
f = v.value();
break;
}
@@ -56,7 +59,7 @@ public class ArmController extends CyberarmState {
@Override
public void buttonDown(Gamepad gamepad, String button) {
if (gamepad != engine.gamepad2) {
if (gamepad != controller) {
return;
}
@@ -64,17 +67,55 @@ public class ArmController extends CyberarmState {
case "guide":
robot.reloadConfig();
break;
// Arm control
case "a":
robot.armPosition(Robot.ArmPosition.COLLECT);
break;
case "x":
robot.armPosition(Robot.ArmPosition.GROUND);
robot.armPosition(Robot.ArmPosition.LOW); // DISABLED GROUND JUNCTION
break;
case "b":
robot.armPosition(Robot.ArmPosition.LOW);
robot.armPosition(Robot.ArmPosition.MEDIUM);
break;
case "y":
robot.armPosition(Robot.ArmPosition.MEDIUM);
robot.armPosition(Robot.ArmPosition.HIGH);
break;
// Manual stepping arm
case "left_bumper":
robot.arm.setTargetPosition(
robot.arm.getCurrentPosition() - robot.angleToTicks(5.0)
);
break;
case "right_bumper":
robot.arm.setTargetPosition(
robot.arm.getCurrentPosition() + robot.angleToTicks(5.0)
);
break;
// Collector control
case "dpad_up":
robot.collectorLeftServo.setPower(1);
robot.collectorRightServo.setPower(1);
break;
case "dpad_down":
robot.collectorLeftServo.setPower(-1);
robot.collectorRightServo.setPower(-1);
}
}
@Override
public void buttonUp(Gamepad gamepad, String button) {
if (gamepad != controller) {
return;
}
switch (button) {
// Collector control
case "dpad_up":
case "dpad_down":
robot.collectorLeftServo.setPower(0);
robot.collectorRightServo.setPower(0);
break;
}
}

118
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/states/teleop/DriveController.java Normal file
View File

@@ -0,0 +1,118 @@
package org.timecrafters.minibots.cyberarm.phoenix.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.phoenix.Robot;
public class DriveController extends CyberarmState {
private final Robot robot;
private final String groupName, actionName;
private final Gamepad controller;
private boolean robotSlowMode;
public DriveController(Robot robot, String groupName, String actionName) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
this.controller = engine.gamepad1;
this.robotSlowMode = true;
}
@Override
public void exec() {
move();
}
@Override
public void stop() {
stopDrive();
}
@Override
public void buttonDown(Gamepad gamepad, String button) {
if (gamepad != controller) {
return;
}
// DEBUG: Toggle hardware fault
if (button.equals("guide")) {
robot.hardwareFault = !robot.hardwareFault;
if (robot.hardwareFault) {
robot.hardwareFaultMessage = "Manually triggered.";
} else {
robot.hardwareFaultMessage = "";
}
}
if (button.equals("right_stick_button")) {
robotSlowMode = !robotSlowMode;
}
if (button.equals("left_stick_button") && robot.hardwareFault) {
robot.imu.resetYaw();
}
}
// REF: https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html
private void move() {
if (robot.automaticAntiTipActive || robot.hardwareFault) {
return;
}
double x = -controller.left_stick_x;
double y = -controller.left_stick_y;
// Improve control?
if (y < 0) {
y = -Math.sqrt(-y);
} else {
y = Math.sqrt(y);
}
if (x < 0) {
x = -Math.sqrt(-x) * 1.1; // Counteract imperfect strafing;
} else {
x = Math.sqrt(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);
double frontLeftPower = 0, frontRightPower = 0, backLeftPower = 0 , backRightPower = 0;
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;
backRightPower = (rotY + rotX - rx) / denominator;
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;
// 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.setVelocity(0); robot.backLeftDrive.setPower(0);
robot.frontRightDrive.setVelocity(0); robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setVelocity(0); robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setVelocity(0); robot.backRightDrive.setPower(0);
}
}