Skip to content

Commit 1725734

Browse files
AndreiDurleaAndreiDurlea
committed
Updated motor test opmodes
They now browse through the hardware map directly
1 parent 4c237fa commit 1725734

2 files changed

Lines changed: 105 additions & 37 deletions

File tree

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestDcMotor.kt‎

Lines changed: 61 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -1,35 +1,80 @@
11
package org.firstinspires.ftc.teamcode
22

3-
import com.andreidurlea.corecontrol.buttonsFeature.AnalogButton
3+
import com.andreidurlea.corecontrol.buttonsFeature.Button
44
import com.andreidurlea.corecontrol.miscellaneous.telemetryEverywhere
55
import com.andreidurlea.corecontrol.opModesFeature.CoreTeleOp
66
import com.qualcomm.robotcore.eventloop.opmode.TeleOp
77
import com.qualcomm.robotcore.hardware.DcMotor
8-
import com.qualcomm.robotcore.hardware.DcMotorSimple
8+
import com.qualcomm.robotcore.hardware.DcMotor.ZeroPowerBehavior
99

10-
@TeleOp(name = "Test Motor", group = "Testing")
10+
@TeleOp(name = "Test DcMotor", group = "Testing")
1111
class TestDcMotor : CoreTeleOp() {
1212

13-
lateinit var motor: DcMotor
13+
lateinit var motors: List<DcMotor>
14+
var currentMotorIndex = 0
15+
var direction = 1.0
1416

15-
var motorName = "EDIT THIS TO CHOOSE YOUR SERVO NAME FROM THE CONFIG"
16-
// you can find and edits configs in TeamCode/res/xml
17-
// or in the Driver Station in the three dots menu > "configure robot"
17+
val reverseDirection = Button({ gamepad1.right_bumper })
1818

19-
val spin = AnalogButton({ gamepad1.right_trigger })
20-
val spinInReverse = AnalogButton({ gamepad1.left_trigger })
19+
val nextMotor = Button({ gamepad1.dpad_right })
20+
val prevMotor = Button({ gamepad1.dpad_left })
2121

2222
override fun onInit() {
23-
motor = hardwareMap[DcMotor::class.java, motorName]
24-
motor.direction = DcMotorSimple.Direction.FORWARD // Starts robot in forward (CW) direction
23+
motors = hardwareMap.getAll(DcMotor::class.java)
24+
motors.forEach { it.zeroPowerBehavior = ZeroPowerBehavior.FLOAT }
25+
telemetryEverywhere.addLine("To add a motor in your config, see the /TeamCode/res/xml folder" +
26+
", or, tap the three dot menu on your driver station and select 'Configure robot'")
2527
}
2628

2729
override fun onLoop() {
28-
telemetryEverywhere.addLine("Spin (Clowckwise) == Right Trigger (0-100%)")
29-
telemetryEverywhere.addLine("Spin (Counterclockwise) == Left Trigger (0-100%)")
30+
if (motors.isEmpty()) {
31+
telemetryEverywhere.addLine("No DC motors found.")
32+
return
33+
}
3034

31-
telemetryEverywhere.addData("Currently testing", motorName)
35+
val previousMotorIndex = currentMotorIndex
36+
if (nextMotor.wasPressed()) {
37+
currentMotorIndex = (currentMotorIndex + 1) % motors.size
38+
}
39+
if (prevMotor.wasPressed()) {
40+
currentMotorIndex = (currentMotorIndex - 1 + motors.size) % motors.size
41+
}
42+
if (previousMotorIndex != currentMotorIndex) {
43+
motors[previousMotorIndex].power = 0.0
44+
}
3245

33-
motor.power = spin.value - spinInReverse.value
46+
47+
val currentMotor = motors[currentMotorIndex]
48+
val motorName = hardwareMap.getNamesOf(currentMotor).firstOrNull() ?: "Unknown"
49+
50+
if (reverseDirection.wasPressed()) {
51+
direction *= -1.0
52+
}
53+
54+
telemetryEverywhere.addLine("[INFO] This OpMode can find and test any motor in your config")
55+
telemetryEverywhere.addLine("[INFO] Control power with Right Trigger")
56+
telemetryEverywhere.addLine("[INFO] Reverse direction with Right Bumper")
57+
telemetryEverywhere.addLine("[INFO] Hold Left Bumper to brake the motor")
58+
telemetryEverywhere.addLine("[INFO] Cycle through motors in your config == D-Pad Left/Right")
59+
telemetryEverywhere.addLine("====================================")
60+
telemetryEverywhere.addData("Currently testing", "MOTOR ${currentMotorIndex + 1}/${motors.size}, named ${motorName.uppercase()}")
61+
telemetryEverywhere.addData("Motor Power", currentMotor.power)
62+
telemetryEverywhere.addData("Motor Direction",
63+
if (direction > 0) "FORWARD (Positive power spins clockwise)"
64+
else "REVERSE (Negative power spins counter-clockwise)")
65+
telemetryEverywhere.addLine("====================================")
66+
67+
if (gamepad1.left_bumper) {
68+
currentMotor.power = 0.0
69+
currentMotor.zeroPowerBehavior = ZeroPowerBehavior.BRAKE
70+
} else {
71+
currentMotor.zeroPowerBehavior = ZeroPowerBehavior.FLOAT
72+
val power = gamepad1.right_trigger * direction
73+
if (power > 0.05 || power < -0.05) {
74+
currentMotor.power = power
75+
} else {
76+
currentMotor.power = 0.0
77+
}
78+
}
3479
}
35-
}
80+
}

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestServo.kt‎

Lines changed: 44 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -9,51 +9,74 @@ import com.qualcomm.robotcore.hardware.Servo
99
@TeleOp(name = "Test Servo", group = "Testing")
1010
class TestServo : CoreTeleOp() {
1111

12-
lateinit var servo: Servo
13-
14-
var servoName = "EDIT THIS TO CHOOSE YOUR SERVO NAME FROM THE CONFIG"
15-
// you can find and edits configs in TeamCode/res/xml
16-
// or in the Driver Station in the three dots menu > "configure robot"
12+
lateinit var servos: List<Servo>
13+
var currentServoIndex = 0
1714

1815
val increasePos = Button({ gamepad1.right_bumper })
1916
val increasePosByALot = Button({ gamepad1.right_trigger > 0.5 })
2017
val decreasePos = Button({ gamepad1.left_bumper })
2118
val decreasePosByALot = Button({ gamepad1.left_trigger > 0.5 })
2219

20+
val nextServo = Button({ gamepad1.dpad_right })
21+
val prevServo = Button({ gamepad1.dpad_left })
22+
2323
override fun onInit() {
24-
servo = hardwareMap[Servo::class.java, servoName]
25-
servo.position = 0.5
24+
servos = hardwareMap.getAll(Servo::class.java)
25+
if (servos.isNotEmpty()) {
26+
servos[currentServoIndex].position = 0.5
27+
}
2628
}
2729

2830
override fun onLoop() {
29-
telemetryEverywhere.addLine("Init starts servo at 0.5")
31+
if (servos.isEmpty()) {
32+
telemetryEverywhere.addLine("[INFO] No servos found in config.")
33+
telemetryEverywhere.addLine("To add a servo in your config, see the /TeamCode/res/xml folder" +
34+
", or, tap the three dot menu on your driver station and select \'Configure robot\'")
35+
return
36+
}
37+
38+
if (nextServo.wasPressed()) {
39+
currentServoIndex = (currentServoIndex + 1) % servos.size
40+
}
41+
if (prevServo.wasPressed()) {
42+
currentServoIndex = (currentServoIndex - 1 + servos.size) % servos.size
43+
}
44+
45+
val currentServo = servos[currentServoIndex]
46+
val servoName = hardwareMap.getNamesOf(currentServo).firstOrNull() ?: "Unknown"
47+
48+
telemetryEverywhere.addLine("[INFO] This OpMode can find and test any servo in your config")
49+
telemetryEverywhere.addLine("[INFO] Init starts servo at 0.5")
3050

31-
telemetryEverywhere.addLine("Increment by 0.01 == Right Bumper")
32-
telemetryEverywhere.addLine("Increment by 0.1 == Right Trigger (>50%)")
51+
telemetryEverywhere.addLine("[INFO] Increment by 0.01 == Right Bumper")
52+
telemetryEverywhere.addLine("[INFO] Increment by 0.1 == Right Trigger (>50%)")
3353

34-
telemetryEverywhere.addLine("Decrement by 0.01 == Left Bumper")
35-
telemetryEverywhere.addLine("Decrement by 0.1 == Left Trigger (>50%)")
54+
telemetryEverywhere.addLine("[INFO] Decrement by 0.01 == Left Bumper")
55+
telemetryEverywhere.addLine("[INFO] Decrement by 0.1 == Left Trigger (>50%)")
3656

37-
telemetryEverywhere.addData("Servo Position", servo.position)
38-
telemetryEverywhere.addData("Currently testing", servoName)
57+
telemetryEverywhere.addLine("[INFO] Cycle through servos in your config == D-Pad Left/Right")
58+
telemetryEverywhere.addLine("====================================")
59+
telemetryEverywhere.addData("Currently testing", "SERVO ${currentServoIndex + 1}/${servos.size}, named ${servoName.uppercase()}")
60+
telemetryEverywhere.addData("Servo Position", currentServo.position)
61+
telemetryEverywhere.addLine("====================================")
3962

4063
if (increasePos.wasPressed()) {
41-
servo.position += 0.01
64+
currentServo.position += 0.01
4265
}
4366

4467
if (increasePosByALot.wasPressed()) {
45-
servo.position = ((servo.position * 10).toInt() + 1) / 10.0
68+
currentServo.position = ((currentServo.position * 10).toInt() + 1) / 10.0
4669
}
4770

4871
if (decreasePos.wasPressed()) {
49-
servo.position -= 0.01
72+
currentServo.position -= 0.01
5073
}
5174

5275
if (decreasePosByALot.wasPressed()) {
53-
if ((servo.position * 10) == (servo.position * 10).toInt().toDouble()) {
54-
servo.position -= 0.1
76+
if ((currentServo.position * 10) == (currentServo.position * 10).toInt().toDouble()) {
77+
currentServo.position -= 0.1
5578
}
56-
servo.position = ((servo.position * 10).toInt()) / 10.0
79+
currentServo.position = ((currentServo.position * 10).toInt()) / 10.0
5780
}
5881
}
59-
}
82+
}

0 commit comments

Comments
 (0)