deep/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestBNO055Enh.java at master · Beta8397/deep · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.util.ElapsedTime;

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.Orientation;
import org.firstinspires.ftc.teamcode.i2c.BNO055Enh;
import org.firstinspires.ftc.teamcode.i2c.BNO055EnhImpl;
import org.firstinspires.ftc.teamcode.util.Toggle;

@TeleOp
@Disabled
public class TestBNO055Enh extends LinearOpMode {

    BNO055EnhImpl imu;

    BNO055Enh.AxesMap aMap = BNO055Enh.AxesMap.XYZ;
    BNO055Enh.AxesSign aSign = BNO055Enh.AxesSign.PPP;

    Toggle toggleA1 = new Toggle(()->gamepad1.a);
    Toggle toggleB1 = new Toggle(()->gamepad1.b);
    Toggle toggleX1 = new Toggle(()->gamepad1.x);

    public void runOpMode(){

        imu = hardwareMap.get(BNO055EnhImpl.class, "imu");

        while (opModeInInit()){
            telemetry.addData("Press A for Map, B for Sign","");
            if (toggleA1.update()){
                aMap = BNO055Enh.AxesMap.values()[(aMap.ordinal()+1)%6];
            }
            if (toggleB1.update()){
                aSign = BNO055Enh.AxesSign.values()[(aSign.ordinal()+1)%8];
            }
            telemetry.addData("AxesMap", aMap);
            telemetry.addData("AxesSign", aSign);
            telemetry.update();
        }

        BNO055Enh.Parameters parameters = new BNO055Enh.Parameters();
        parameters.axesMap = aMap;
        parameters.axesSign = aSign;

        imu.initialize(parameters);
        ElapsedTime delay = new ElapsedTime();
        while (opModeIsActive() && delay.milliseconds()<3000) continue;

        while (opModeIsActive()){

            Orientation orientation = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);

            telemetry.addData("ZYX angles","%.1f  %.1f  %.1f", orientation.firstAngle,
                    orientation.secondAngle, orientation.thirdAngle);

            telemetry.update();
        }

    }
}