Skip to content

Commit fbb6de8

Browse files
committed
Added some stuff for testing turret.
1 parent dd6e48a commit fbb6de8

File tree

3 files changed

+10
-1
lines changed

3 files changed

+10
-1
lines changed

TeamCode/src/main/kotlin/pioneer/hardware/Turret.kt

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -63,6 +63,9 @@ class Turret(
6363
val targetAngle: Double
6464
get() = turret.targetPosition / ticksPerRadian
6565

66+
val targetTicks: Int
67+
get() = turret.targetPosition
68+
6669
fun resetMotorPosition(resetTicks: Int = 0) {
6770
turret.mode = DcMotor.RunMode.STOP_AND_RESET_ENCODER
6871
offsetTicks = resetTicks

TeamCode/src/main/kotlin/pioneer/opmodes/calibration/ServoCalibration.kt

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,6 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode
55
import com.qualcomm.robotcore.eventloop.opmode.TeleOp
66
import com.qualcomm.robotcore.hardware.Servo
77

8-
@Disabled
98
@TeleOp(name = "Servo Calibration", group = "Calibration")
109
class ServoCalibration : OpMode() {
1110
lateinit var servo: Servo

TeamCode/src/main/kotlin/pioneer/opmodes/teleop/Teleop.kt

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
package pioneer.opmodes.teleop
22

3+
import com.acmerobotics.dashboard.FtcDashboard
34
import com.qualcomm.robotcore.eventloop.opmode.TeleOp
45
import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit
56
import pioneer.Bot
@@ -64,11 +65,17 @@ class Teleop : BaseOpMode() {
6465
telemetry.addData("Flywheel Speed", driver2.flywheelVelocityEnum)
6566
telemetry.addData("Flywheel TPS", bot.flywheel?.velocity)
6667
telemetry.addData("Turret Angle", driver2.turretAngle)
68+
telemetry.addData("Turret Target Ticks", bot.turret?.targetTicks)
69+
telemetry.addData("Turret Real Ticks", bot.turret?.currentTicks)
6770
telemetry.addData("Drive Power", driver1.drivePower)
6871
telemetry.addData("Field Centric", driver1.fieldCentric)
6972
telemetry.addData("Velocity", "vx: %.2f, vy: %.2f".format(bot.pinpoint?.pose?.vx, bot.pinpoint?.pose?.vy))
7073
telemetry.addData("Voltage", bot.batteryMonitor?.voltage)
7174
telemetry.addData("Flywheel Motor Current", bot.flywheel?.motor?.getCurrent(CurrentUnit.MILLIAMPS))
7275
telemetryPacket.addLine("Flywheel TPS" + (bot.flywheel?.velocity ?: 0.0))
76+
77+
telemetryPacket.put("Turret Target Ticks", bot.turret?.targetTicks)
78+
telemetryPacket.put("Turret Current Ticks", bot.turret?.currentTicks)
79+
7380
}
7481
}

0 commit comments

Comments
 (0)