missing
8cdd3dd447
most important stuff: kinematics, autons, config, dashboard, easyopencv + april tags, and probably more im forgetting https://xkcd.com/1296/
119 lines
4.5 KiB
Java
119 lines
4.5 KiB
Java
package org.firstinspires.ftc.teamcode;
|
|
|
|
import com.acmerobotics.dashboard.FtcDashboard;
|
|
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
|
import com.qualcomm.hardware.lynx.LynxModule;
|
|
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
|
|
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
|
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
|
import com.qualcomm.robotcore.hardware.HardwareMap;
|
|
import com.qualcomm.robotcore.hardware.IMU;
|
|
import com.qualcomm.robotcore.hardware.Servo;
|
|
|
|
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
|
|
|
import java.util.List;
|
|
|
|
public class Hardware {
|
|
public final OpMode opMode;
|
|
public final DashboardTelemetryWrapper dashboardTelemetryWrapper;
|
|
public final DcMotor fl;
|
|
public final DcMotor fr;
|
|
public final DcMotor bl;
|
|
public final DcMotor br;
|
|
public final DcMotor spool;
|
|
public final Servo clawLeft;
|
|
public final Servo clawRight;
|
|
public final Servo wrist;
|
|
public final IMU imu;
|
|
public final List<LynxModule> hubs;
|
|
|
|
public Hardware(OpMode opMode) {
|
|
HardwareMap hm = opMode.hardwareMap;
|
|
this.opMode = opMode;
|
|
this.dashboardTelemetryWrapper = new DashboardTelemetryWrapper(FtcDashboard.getInstance());
|
|
opMode.telemetry = new MultipleTelemetry(opMode.telemetry, this.dashboardTelemetryWrapper);
|
|
|
|
this.fl = hm.get(DcMotor.class, "fl");
|
|
this.fr = hm.get(DcMotor.class, "fr");
|
|
this.bl = hm.get(DcMotor.class, "bl");
|
|
this.br = hm.get(DcMotor.class, "br");
|
|
|
|
this.fl.setDirection(DcMotorSimple.Direction.REVERSE);
|
|
this.bl.setDirection(DcMotorSimple.Direction.REVERSE);
|
|
this.fr.setDirection(DcMotorSimple.Direction.FORWARD);
|
|
this.br.setDirection(DcMotorSimple.Direction.FORWARD);
|
|
this.fl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
|
this.fr.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
|
this.bl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
|
this.br.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
|
this.fl.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
|
this.fr.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
|
this.bl.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
|
this.br.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
|
|
|
this.spool = hm.get(DcMotor.class, "spool");
|
|
this.clawLeft = hm.get(Servo.class, "clawLeft");
|
|
this.clawRight = hm.get(Servo.class, "clawRight");
|
|
this.wrist = hm.get(Servo.class, "wrist");
|
|
this.imu = hm.get(IMU.class, "imu");
|
|
// this.posImu = hm.get(BNO055IMU.class, "imu");
|
|
|
|
this.spool.setDirection(DcMotorSimple.Direction.FORWARD);
|
|
this.spool.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
|
|
|
this.clawLeft.scaleRange(0, 1/3.);
|
|
this.clawRight.scaleRange(2/3., 1);
|
|
this.wrist.scaleRange(0.25, 1);
|
|
|
|
this.hubs = hm.getAll(LynxModule.class);
|
|
for (LynxModule hub : this.hubs) {
|
|
hub.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
|
|
}
|
|
}
|
|
|
|
public void setClawPosition(double pos) {
|
|
this.clawLeft.setPosition(1 - pos);
|
|
this.clawRight.setPosition(pos);
|
|
}
|
|
|
|
public void setWristPosition(double pos) {
|
|
this.wrist.setPosition((pos + 1) / 2);
|
|
}
|
|
|
|
public void initIMU() {
|
|
IMU.Parameters parameters = new IMU.Parameters(
|
|
new RevHubOrientationOnRobot(
|
|
RevHubOrientationOnRobot.LogoFacingDirection.FORWARD,
|
|
RevHubOrientationOnRobot.UsbFacingDirection.LEFT
|
|
)
|
|
);
|
|
|
|
this.imu.initialize(parameters);
|
|
}
|
|
|
|
// public void initPosIMU(BNO055IMU.AccelerationIntegrator integrator) {
|
|
// BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
|
|
// parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
|
|
// parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
|
|
// parameters.calibrationDataFile = "AdafruitIMUCalibration.json";
|
|
// parameters.accelerationIntegrationAlgorithm = integrator;
|
|
//
|
|
// this.posImu.initialize(parameters);
|
|
// this.posImu.stopAccelerationIntegration();
|
|
// }
|
|
|
|
public double getImuYaw() {
|
|
return -this.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
|
|
}
|
|
|
|
public int getLiftEncoder() {
|
|
return -spool.getCurrentPosition();
|
|
}
|
|
|
|
// public FieldVector getIMUPosition() {
|
|
// Position tmp = this.posImu.getPosition().toUnit(DistanceUnit.MM);
|
|
// return FieldVector.inMM(tmp.x, tmp.y);
|
|
// }
|
|
} |