59 lines
2.3 KiB
Java
59 lines
2.3 KiB
Java
package org.firstinspires.ftc.teamcode;
|
|
|
|
import com.qualcomm.hardware.bosch.BNO055IMU;
|
|
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.Servo;
|
|
|
|
import org.firstinspires.ftc.robotcore.external.navigation.Position;
|
|
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
|
|
|
|
public class Hardware {
|
|
public final DcMotor fl;
|
|
public final DcMotor fr;
|
|
public final DcMotor bl;
|
|
public final DcMotor br;
|
|
public final DcMotor spool;
|
|
public final Servo claw;
|
|
public final Servo wrist;
|
|
public final BNO055IMU imu;
|
|
|
|
public Hardware(OpMode opMode) {
|
|
this(opMode.hardwareMap);
|
|
}
|
|
|
|
public Hardware(HardwareMap hardwareMap) {
|
|
this.fl = hardwareMap.get(DcMotor.class, "fl");
|
|
this.fr = hardwareMap.get(DcMotor.class, "fr");
|
|
this.bl = hardwareMap.get(DcMotor.class, "bl");
|
|
this.br = hardwareMap.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.spool = hardwareMap.get(DcMotor.class, "spool");
|
|
this.claw = hardwareMap.get(Servo.class, "claw");
|
|
this.wrist = hardwareMap.get(Servo.class, "wrist");
|
|
this.imu = hardwareMap.get(BNO055IMU.class, "imu");
|
|
|
|
this.wrist.scaleRange(0.25, 1);
|
|
}
|
|
|
|
public void initIMU() {
|
|
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
|
|
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
|
|
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
|
|
parameters.calibrationDataFile = "BNO055IMUCalibration.json";
|
|
|
|
this.imu.initialize(parameters);
|
|
this.imu.startAccelerationIntegration(new Position(), new Velocity(), 1);
|
|
}
|
|
} |