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

Compare commits

base: TimeCrafters:1b148ad75b0352cd31ea72e5b9d3310f8d693a95
Branches Tags
TimeCrafters:master
..
compare: TimeCrafters:master
Branches Tags
TimeCrafters:master

3 Commits
1b148ad75b ... master

Author SHA1 Message Date
NerdyBirdy460
ad83dc5e0c Merge remote-tracking branch 'origin/master' 2023-03-25 12:08:19 -05:00
NerdyBirdy460
151c866ade Minibot program- day 3 2023-03-25 12:08:04 -05:00
Cyberarm
fd3d6cb44d Changes 2023-03-25 11:36:43 -05:00
6 changed files with 81 additions and 38 deletions
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; workingPosition += estimatedTravel;
travelling = true; travelling = true;
} else { } else {
workingPosition = targetPosition;
travelling = false; travelling = false;
} }
@@ -68,10 +69,16 @@ public class PositionalServoController {
return estimatedPosition; return estimatedPosition;
} }
public double getWorkingPosition() { return workingPosition; }
public double getTargetPosition() { return targetPosition; }
public double getEstimatedAngle() { public double getEstimatedAngle() {
return estimatedPosition * servoRangeInDegrees; return estimatedPosition * servoRangeInDegrees;
} }
public double getError() { return targetPosition - estimatedPosition; }
private double lerp(double min, double max, double t) private double lerp(double min, double max, double t)
{ {
return min + (max - min) * 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.setTargetPosition(0);
arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
armTargetPosition = ArmPosition.COLLECT;
// ZERO POWER BEHAVIOR // ZERO POWER BEHAVIOR
frontLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); frontLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@@ -148,16 +149,21 @@ public class Robot {
arm.setPower(tuningConfig("arm_automatic_power").value()); arm.setPower(tuningConfig("arm_automatic_power").value());
// SERVOS // 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 // 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( leftRiserServoController = new PositionalServoController(
engine.hardwareMap.servo.get("LowRiserLeft"), leftRiser,
5.0, 5.0,
428.57142858, 428.57142858,
270.0 270.0
); // SERVER PORT: ? - ? HUB ); // SERVER PORT: ? - ? HUB
rightRiserServoController = new PositionalServoController( rightRiserServoController = new PositionalServoController(
engine.hardwareMap.servo.get("LowRiserRight"), rightRiser,
5.0, 5.0,
428.57142858, 428.57142858,
270.0 270.0
@@ -176,9 +182,11 @@ public class Robot {
// SENSORS // SENSORS
// IMU // IMU
IMU.Parameters parameters = new IMU.Parameters( IMU.Parameters parameters = new IMU.Parameters(
new RevHubOrientationOnRobot( new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP, RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD,
RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD)); RevHubOrientationOnRobot.UsbFacingDirection.UP
)
);
imu.initialize(parameters); 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.addData(" Arm", "%d (%8.2f degrees)", arm.getTargetPosition(), ticksToAngle(arm.getTargetPosition()));
engine.telemetry.addLine();
// Motor Velocity // Motor Velocity
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())); 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 // Servo Positions
engine.telemetry.addLine("Servo Positions"); engine.telemetry.addLine("Servo Positions");
engine.telemetry.addData("Left Riser (Est)", leftRiserServoController.getEstimatedPosition()); 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)", rightRiserServoController.getEstimatedPosition()); 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.addData(" Camera (Blind)", cameraServo.getPosition());
engine.telemetry.addLine(); engine.telemetry.addLine();
// Continuous Servo Powers // Continuous Servo Powers
engine.telemetry.addLine("Servo Powers"); engine.telemetry.addLine("Servo Powers");
engine.telemetry.addData("Collector Left", collectorLeftServo.getPower()); engine.telemetry.addData(" Collector Left", collectorLeftServo.getPower());
engine.telemetry.addData("Collector Right", collectorRightServo.getPower()); engine.telemetry.addData(" Collector Right", collectorRightServo.getPower());
engine.telemetry.addLine(); engine.telemetry.addLine();
// Servo Directions // Servo Directions
engine.telemetry.addLine("Servo Directions"); engine.telemetry.addLine("Servo Directions");
engine.telemetry.addData("Left Riser", leftRiserServoController.getServo().getDirection()); engine.telemetry.addData(" Left Riser", leftRiserServoController.getServo().getDirection());
engine.telemetry.addData("Right Riser", rightRiserServoController.getServo().getDirection()); engine.telemetry.addData(" Right Riser", rightRiserServoController.getServo().getDirection());
engine.telemetry.addData("Camera", cameraServo.getDirection()); engine.telemetry.addData(" Camera", cameraServo.getDirection());
engine.telemetry.addData("Collector Left", collectorLeftServo.getDirection()); engine.telemetry.addData(" Collector Left", collectorLeftServo.getDirection());
engine.telemetry.addData("Collector Right", collectorRightServo.getDirection()); engine.telemetry.addData(" Collector Right", collectorRightServo.getDirection());
engine.telemetry.addLine(); 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 // Collector control
case "dpad_up": case "dpad_up":
robot.collectorLeftServo.setPower(1);
robot.collectorRightServo.setPower(1);
break;
case "dpad_down":
robot.collectorLeftServo.setPower(-1); robot.collectorLeftServo.setPower(-1);
robot.collectorRightServo.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; return;
} }
double x = -controller.left_stick_x; double x = controller.left_stick_x;
double y = -controller.left_stick_y; double y = -controller.left_stick_y;
// Improve control? // Improve control?

21
TeamCode/src/main/java/org/timecrafters/minibots/states/Mini2023Bot.java
View File

@@ -1,7 +1,10 @@
package org.timecrafters.minibots.states; package org.timecrafters.minibots.states;
import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cRangeSensor;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.IMU;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import org.timecrafters.minibots.engines.Mini2023Engine; import org.timecrafters.minibots.engines.Mini2023Engine;
@@ -11,8 +14,10 @@ public class Mini2023Bot {
private final Mini2023Engine engine; private final Mini2023Engine engine;
public TimeCraftersConfiguration configuration; public TimeCraftersConfiguration configuration;
public DcMotor Opportunity, Spirit, Victoria, Endeavour; //Don't ask. Just don't. public DcMotor Opportunity, Spirit; //Don't ask. Just don't.
public CRServo servoA, servoB, servoC; //Just be thankful the servos have 'normal' names. public CRServo servoA, servoB, servoC; //Just be thankful the servos have 'normal' names.
public IMU imu;
public ModernRoboticsI2cRangeSensor hazcam;
public Mini2023Bot(Mini2023Engine engine) { public Mini2023Bot(Mini2023Engine engine) {
this.engine = engine; this.engine = engine;
@@ -21,15 +26,21 @@ public class Mini2023Bot {
private void setupRobot() { private void setupRobot() {
Opportunity = engine.hardwareMap.get(DcMotor.class, "Left Wheel"); Spirit = engine.hardwareMap.get(DcMotor.class, "Left Wheel");
Spirit = engine.hardwareMap.get(DcMotor.class, "Right Wheel"); Opportunity = engine.hardwareMap.get(DcMotor.class, "Right Wheel");
// Victoria = engine.hardwareMap.get(DcMotor.class, "Rear Dead Wheel");
// Endeavour = engine.hardwareMap.get(DcMotor.class, "Front Dead Wheel");
servoA = engine.hardwareMap.get(CRServo.class, "Servo 1"); servoA = engine.hardwareMap.get(CRServo.class, "Servo 1");
servoB = engine.hardwareMap.get(CRServo.class, "Servo 2"); servoB = engine.hardwareMap.get(CRServo.class, "Servo 2");
servoC = engine.hardwareMap.get(CRServo.class, "Servo 3"); servoC = engine.hardwareMap.get(CRServo.class, "Servo 3");
IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD,
RevHubOrientationOnRobot.UsbFacingDirection.UP
));
this.imu = engine.hardwareMap.get(IMU.class, "imu");
imu.initialize(parameters);
// configuration = new TimeCraftersConfiguration("2023 Mini"); // configuration = new TimeCraftersConfiguration("2023 Mini");

41
TeamCode/src/main/java/org/timecrafters/minibots/states/Mini2023State.java
View File

@@ -9,51 +9,66 @@ public class Mini2023State extends CyberarmState {
public double lThrust, rThrust, orbitSpeed; public double lThrust, rThrust, orbitSpeed;
public double deltaServo;
public Mini2023State(Mini2023Bot robot) {this.robot = robot;} public Mini2023State(Mini2023Bot robot) {this.robot = robot;}
public double getDeltaServo() {
return deltaServo;
}
@Override public void telemetry() {
engine.telemetry.addData("Right Drive Power", robot.Opportunity.getPower());
engine.telemetry.addData("Left Drive Power", robot.Spirit.getPower());
}
@Override @Override
public void init() { public void init() {
lThrust = 0; lThrust = 0;
rThrust = 0; rThrust = 0;
orbitSpeed = 0; orbitSpeed = 0;
robot.servoA.setPower(0); robot.servoA.setPower(orbitSpeed);
robot.servoB.setPower(0); robot.servoB.setPower(orbitSpeed);
robot.servoC.setPower(0); robot.servoC.setPower(orbitSpeed);
robot.Opportunity.setPower(lThrust); robot.Spirit.setPower(lThrust);
robot.Spirit.setPower(rThrust); robot.Opportunity.setPower(rThrust);
} }
@Override @Override
public void exec() { public void exec() {
if (Math.abs(engine.gamepad1.left_stick_y) > 0.1) { if (Math.abs(engine.gamepad1.left_stick_y) > 0.1) {
lThrust = engine.gamepad2.left_stick_y; lThrust = engine.gamepad1.left_stick_y * 0.5;
robot.Opportunity.setPower(lThrust); robot.Spirit.setPower(lThrust);
} else { } else {
lThrust = 0; lThrust = 0;
robot.Opportunity.setPower(lThrust); robot.Spirit.setPower(lThrust);
} }
if (Math.abs(engine.gamepad1.right_stick_y) > 0.1) { if (Math.abs(engine.gamepad1.right_stick_y) > 0.1) {
rThrust = engine.gamepad1.right_stick_y; rThrust = engine.gamepad1.right_stick_y * 0.5;
robot.Spirit.setPower(rThrust); robot.Opportunity.setPower(rThrust);
} else { } else {
rThrust = 0; rThrust = 0;
robot.Spirit.setPower(rThrust); robot.Opportunity.setPower(rThrust);
} }
if (engine.gamepad1.right_trigger > 0.1) { if (engine.gamepad1.right_trigger > 0.1) {
orbitSpeed = engine.gamepad1.right_trigger * 0.75; orbitSpeed = engine.gamepad1.right_trigger * 0.5;
robot.servoA.setPower(orbitSpeed); robot.servoA.setPower(orbitSpeed);
robot.servoB.setPower(orbitSpeed); robot.servoB.setPower(orbitSpeed);
robot.servoC.setPower(orbitSpeed); robot.servoC.setPower(orbitSpeed);
} else if (engine.gamepad1.left_trigger > 0.1) { } else if (engine.gamepad1.left_trigger > 0.1) {
orbitSpeed = engine.gamepad1.left_trigger * 0.75; orbitSpeed = engine.gamepad1.left_trigger * 0.5;
robot.servoA.setPower(orbitSpeed); robot.servoA.setPower(orbitSpeed);
robot.servoB.setPower(orbitSpeed); robot.servoB.setPower(orbitSpeed);
robot.servoC.setPower(orbitSpeed); robot.servoC.setPower(orbitSpeed);
} else { } else {
orbitSpeed = 0; orbitSpeed = 0;
robot.servoA.setPower(orbitSpeed);
robot.servoB.setPower(orbitSpeed);
robot.servoC.setPower(orbitSpeed);
} }