Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- package org.firstinspires.ftc.teamcode.Skystone;
- import android.os.SystemClock;
- import com.qualcomm.hardware.bosch.BNO055IMU;
- import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
- import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
- import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
- import com.qualcomm.robotcore.hardware.DcMotor;
- import com.qualcomm.robotcore.hardware.HardwareMap;
- import com.qualcomm.robotcore.hardware.Servo;
- import com.qualcomm.robotcore.util.Range;
- import org.firstinspires.ftc.robotcore.external.Telemetry;
- import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
- import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
- import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
- import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
- import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
- import org.firstinspires.ftc.robotcore.external.navigation.Position;
- import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
- import java.util.Timer;
- import java.util.Timer;
- import static org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit.CM;
- import static org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit.MM;
- public class Robot {
- //Drive Motors
- public DcMotor fLeft;
- public DcMotor fRight;
- public DcMotor bLeft;
- public DcMotor bRight;
- //imu
- private BNO055IMU imu;
- private Orientation angles;
- private Position position;
- //Inherited classes from Op Mode
- private Telemetry telemetry;
- private HardwareMap hardwareMap;
- private LinearOpMode linearOpMode;
- //PID (concept only)
- public Robot(HardwareMap hardwareMap, Telemetry telemetry, LinearOpMode linearOpMode){
- this.telemetry = telemetry;
- this.hardwareMap = hardwareMap;
- this.linearOpMode = linearOpMode;
- //config names need to match configs on the phone
- //Map drive motors
- fLeft = hardwareMap.dcMotor.get("fLeft");
- fRight = hardwareMap.dcMotor.get("fRight");
- bLeft = hardwareMap.dcMotor.get("bLeft");
- bRight = hardwareMap.dcMotor.get("bRight");
- //Set direction of drive motors
- fLeft.setDirection(DcMotor.Direction.FORWARD);
- fRight.setDirection(DcMotor.Direction.REVERSE);
- bLeft.setDirection(DcMotor.Direction.FORWARD);
- bRight.setDirection(DcMotor.Direction.REVERSE);
- }
- public void setDrivePower(double power){
- fLeft.setPower(power);
- fRight.setPower(power);
- bLeft.setPower(power);
- bRight.setPower(power);
- }
- public void intializeIMU(){
- BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
- parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
- parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
- parameters.calibrationDataFile = "BNO055IMUCalibration.json"; // see the calibration sample opmode
- parameters.loggingEnabled = true;
- parameters.loggingTag = "IMU";
- parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
- imu = hardwareMap.get(BNO055IMU.class, "imu");
- imu.initialize(parameters);
- imu.startAccelerationIntegration(new Position(), new Velocity(), 1000);
- angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.YXZ, AngleUnit.DEGREES);
- }
- public void resetEncoders(){
- fLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
- fRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
- bLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
- bRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
- }
- public void changeRunModeToUsingEncoder(){
- fLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
- fRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
- bLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
- bRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
- }
- public void setMotorMode(DcMotor.RunMode runMode){
- fLeft.setMode(runMode);
- fRight.setMode(runMode);
- bLeft.setMode(runMode);
- bRight.setMode(runMode);
- }
- public double[] getPosUsingIncrements() {
- long startTime = SystemClock.elapsedRealtime();
- double[] positions = new double[3];
- final double radius = 2;
- final double encoderPerRevolution = 537.6;
- final double l = 7;
- final double w = 6.5;
- double xPosGlobal = 0;
- double yPosGlobal = 0;
- double angleGlobal = 0;
- // find robot position
- double fl = 2 * Math.PI * fLeft.getCurrentPosition() / encoderPerRevolution; //radians each motor has travelled
- double fr = 2 * Math.PI * fRight.getCurrentPosition() / encoderPerRevolution;
- double bl = 2 * Math.PI * bLeft.getCurrentPosition() / encoderPerRevolution;
- double br = 2 * Math.PI * bRight.getCurrentPosition() / encoderPerRevolution;
- double xPosRobot = radius/4 * (fl + bl + br + fr);
- double yPosRobot = radius/4 * (-fl + bl - br + fr);
- double angleRobot = radius/4 *(-fl/(l+w) - bl/(l+w) + br/(l+w) + fr/(l+w));
- angleRobot = angleRobot * 180/(2*Math.PI);
- //converting to global frame
- while(linearOpMode.opModeIsActive()){
- long elapsedTime = SystemClock.elapsedRealtime() - startTime;
- //these variables have random names, used to make typing xPosGlobal and yPosGlobal easier
- double angleDeltaRobot = angleRobot * elapsedTime;
- double xDeltaRobot = xPosRobot * elapsedTime;
- double yDeltaRobot = yPosRobot * elapsedTime;
- //finding global position
- xPosGlobal += (Math.cos(angleGlobal) * Math.sin(angleDeltaRobot) - (Math.cos(angleDeltaRobot) - 1) * Math.sin(angleGlobal)) * xDeltaRobot / angleDeltaRobot + (Math.cos(angleGlobal) * (Math.cos(angleDeltaRobot) - 1) - Math.sin(angleGlobal) * Math.sin(angleDeltaRobot)) * yDeltaRobot / angleDeltaRobot;
- yPosGlobal += ((Math.cos(angleDeltaRobot) - 1) * Math.sin(angleGlobal) + (Math.cos(angleGlobal)) * Math.sin(angleDeltaRobot)) * yDeltaRobot / angleDeltaRobot + (Math.cos(angleGlobal) * (Math.cos(angleDeltaRobot) - 1) + Math.sin(angleGlobal) * Math.sin(angleDeltaRobot)) * xDeltaRobot / angleDeltaRobot;
- angleGlobal += angleDeltaRobot;
- //inserting into return array
- positions[0] += xPosGlobal;
- positions[1] += yPosGlobal;
- positions[2] += angleGlobal;
- resetEncoders();
- linearOpMode.sleep(10);
- }
- return positions;
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement