-
Notifications
You must be signed in to change notification settings - Fork 0
/
XDriveBotDemo.java
101 lines (92 loc) · 4.67 KB
/
XDriveBotDemo.java
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
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
package org.firstinspires.ftc.teamcode;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.*;
import org.firstinspires.ftc.robotcore.external.navigation.*;
/**
* Example OpMode. Demonstrates use of gyro, color sensor, encoders, and telemetry.
*
*/
@TeleOp(name = "XDriveBot demo", group = "XBot")
public class XDriveBotDemo extends LinearOpMode {
public void runOpMode(){
DcMotor m1 = hardwareMap.dcMotor.get("back_left_motor");
DcMotor m2 = hardwareMap.dcMotor.get("front_left_motor");
DcMotor m3 = hardwareMap.dcMotor.get("front_right_motor");
DcMotor m4 = hardwareMap.dcMotor.get("back_right_motor");
m1.setDirection(DcMotor.Direction.REVERSE);
m2.setDirection(DcMotor.Direction.REVERSE);
m1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
m2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
m3.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
m4.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
m1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
m2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
m3.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
m4.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
//GyroSensor gyro = hardwareMap.gyroSensor.get("gyro_sensor");
BNO055IMU imu = hardwareMap.get(BNO055IMU.class, "imu");
CRServo backServo = hardwareMap.crservo.get("back_crservo");
DistanceSensor frontDistance = hardwareMap.get(DistanceSensor.class, "front_distance");
DistanceSensor leftDistance = hardwareMap.get(DistanceSensor.class, "left_distance");
DistanceSensor rightDistance = hardwareMap.get(DistanceSensor.class, "right_distance");
DistanceSensor backDistance = hardwareMap.get(DistanceSensor.class, "back_distance");
//gyro.init();
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.accelerationIntegrationAlgorithm = null;
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
parameters.calibrationData = null;
parameters.calibrationDataFile = "";
parameters.loggingEnabled = false;
parameters.loggingTag = "Who cares.";
imu.initialize(parameters);
ColorSensor colorSensor = hardwareMap.colorSensor.get("color_sensor");
telemetry.addData("Press Start When Ready","");
telemetry.update();
waitForStart();
while (opModeIsActive()){
double px = gamepad1.left_stick_x;
if (Math.abs(px) < 0.05) px = 0;
double py = -gamepad1.left_stick_y;
if (Math.abs(py) < 0.05) py = 0;
double pa = -gamepad1.right_stick_x;
if (Math.abs(pa) < 0.05) pa = 0;
double p1 = -px + py - pa;
double p2 = px + py + -pa;
double p3 = -px + py + pa;
double p4 = px + py + pa;
double max = Math.max(1.0, Math.abs(p1));
max = Math.max(max, Math.abs(p2));
max = Math.max(max, Math.abs(p3));
max = Math.max(max, Math.abs(p4));
p1 /= max;
p2 /= max;
p3 /= max;
p4 /= max;
m1.setPower(p1);
m2.setPower(p2);
m3.setPower(p3);
m4.setPower(p4);
double psrv = -gamepad2.left_stick_y;
if (Math.abs(psrv) < 0.05) psrv = 0.0;
backServo.setPower(psrv);
telemetry.addData("Color","R %d G %d B %d", colorSensor.red(), colorSensor.green(), colorSensor.blue());
//telemetry.addData("Heading"," %.1f", gyro.getHeading());
Orientation orientation = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.RADIANS);
telemetry.addData("Heading", " %.1f", orientation.firstAngle * 180.0 / Math.PI);
telemetry.addData("Front Distance", " %.1f", frontDistance.getDistance(DistanceUnit.CM));
telemetry.addData("Left Distance", " %.1f", leftDistance.getDistance(DistanceUnit.CM));
telemetry.addData("Right Distance", " %.1f", rightDistance.getDistance(DistanceUnit.CM));
telemetry.addData("Back Distance", " %.1f", backDistance.getDistance(DistanceUnit.CM));
telemetry.addData("Encoders"," %d %d %d %d", m1.getCurrentPosition(), m2.getCurrentPosition(),
m3.getCurrentPosition(), m4.getCurrentPosition());
telemetry.update();
}
m1.setPower(0);
m2.setPower(0);
m3.setPower(0);
m4.setPower(0);
}
}