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

Changes

Browse Source
This commit is contained in:
Cyberarm
2023-03-25 11:36:43 -05:00
parent 1b148ad75b
commit fd3d6cb44d
4 changed files with 37 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/PositionalServoController.java
View File

@@ -53,6 +53,7 @@ public class PositionalServoController {
workingPosition += estimatedTravel;
travelling = true;
} else {
workingPosition = targetPosition;
travelling = false;
}
@@ -68,10 +69,16 @@ public class PositionalServoController {
return estimatedPosition;
}
public double getWorkingPosition() { return workingPosition; }
public double getTargetPosition() { return targetPosition; }
public double getEstimatedAngle() {
return estimatedPosition * servoRangeInDegrees;
}
public double getError() { return targetPosition - estimatedPosition; }
private double lerp(double min, double max, double t)
{
return min + (max - min) * t;

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

@@ -134,6 +134,7 @@ public class Robot {
arm.setTargetPosition(0);
arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
armTargetPosition = ArmPosition.COLLECT;
// ZERO POWER BEHAVIOR
frontLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@@ -148,16 +149,21 @@ public class Robot {
arm.setPower(tuningConfig("arm_automatic_power").value());
// SERVOS
Servo leftRiser = engine.hardwareMap.servo.get("LowRiserLeft");
leftRiser.setDirection(Servo.Direction.FORWARD);
Servo rightRiser = engine.hardwareMap.servo.get("LowRiserRight");
rightRiser.setDirection(Servo.Direction.REVERSE);
// 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"),
leftRiser,
5.0,
428.57142858,
270.0
); // SERVER PORT: ? - ? HUB
rightRiserServoController = new PositionalServoController(
engine.hardwareMap.servo.get("LowRiserRight"),
rightRiser,
5.0,
428.57142858,
270.0
@@ -176,9 +182,11 @@ public class Robot {
// SENSORS
// IMU
IMU.Parameters parameters = new IMU.Parameters(
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP,
RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD));
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD,
RevHubOrientationOnRobot.UsbFacingDirection.UP
)
);
imu.initialize(parameters);
@@ -286,6 +294,8 @@ public class Robot {
engine.telemetry.addData(" Arm", "%d (%8.2f degrees)", arm.getTargetPosition(), ticksToAngle(arm.getTargetPosition()));
engine.telemetry.addLine();
// Motor Velocity
engine.telemetry.addLine("Motor Velocity");
engine.telemetry.addData(" Front Left Drive", "%8.2f (%8.2f in)", frontLeftDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) frontLeftDrive.getVelocity()));
@@ -330,26 +340,26 @@ public class Robot {
// Servo Positions
engine.telemetry.addLine("Servo Positions");
engine.telemetry.addData("Left Riser (Est)", leftRiserServoController.getEstimatedPosition());
engine.telemetry.addData("Right Riser (Est)", rightRiserServoController.getEstimatedPosition());
engine.telemetry.addData("Camera (Blind)", cameraServo.getPosition());
engine.telemetry.addData(" Left Riser (Est)/(Target)/(Blind)", "%.4f [error: %.4f] / %.4f / %.4f", leftRiserServoController.getEstimatedPosition(), leftRiserServoController.getError(), leftRiserServoController.getTargetPosition(), leftRiserServoController.getServo().getPosition());
engine.telemetry.addData(" Right Riser (Est)/(Target)/(Blind)", "%.4f [error: %.4f] / %.4f / %.4f", rightRiserServoController.getEstimatedPosition(), rightRiserServoController.getError(), rightRiserServoController.getTargetPosition(), rightRiserServoController.getServo().getPosition());
engine.telemetry.addData(" Camera (Blind)", cameraServo.getPosition());
engine.telemetry.addLine();
// Continuous Servo Powers
engine.telemetry.addLine("Servo Powers");
engine.telemetry.addData("Collector Left", collectorLeftServo.getPower());
engine.telemetry.addData("Collector Right", collectorRightServo.getPower());
engine.telemetry.addData(" Collector Left", collectorLeftServo.getPower());
engine.telemetry.addData(" Collector Right", collectorRightServo.getPower());
engine.telemetry.addLine();
// Servo Directions
engine.telemetry.addLine("Servo Directions");
engine.telemetry.addData("Left Riser", leftRiserServoController.getServo().getDirection());
engine.telemetry.addData("Right Riser", rightRiserServoController.getServo().getDirection());
engine.telemetry.addData("Camera", cameraServo.getDirection());
engine.telemetry.addData("Collector Left", collectorLeftServo.getDirection());
engine.telemetry.addData("Collector Right", collectorRightServo.getDirection());
engine.telemetry.addData(" Left Riser", leftRiserServoController.getServo().getDirection());
engine.telemetry.addData(" Right Riser", rightRiserServoController.getServo().getDirection());
engine.telemetry.addData(" Camera", cameraServo.getDirection());
engine.telemetry.addData(" Collector Left", collectorLeftServo.getDirection());
engine.telemetry.addData(" Collector Right", collectorRightServo.getDirection());
engine.telemetry.addLine();

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

@@ -95,12 +95,12 @@ public class ArmController extends CyberarmState {
// 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);
break;
case "dpad_down":
robot.collectorLeftServo.setPower(1);
robot.collectorRightServo.setPower(1);
}
}

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

@@ -65,7 +65,7 @@ public class DriveController extends CyberarmState {
return;
}
double x = -controller.left_stick_x;
double x = controller.left_stick_x;
double y = -controller.left_stick_y;
// Improve control?