Dedededec0de/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.kt at master · team5009/Dedededec0de · 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
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
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
package org.firstinspires.ftc.teamcode

import ca.helios5009.hyperion.core.Motors
import ca.helios5009.hyperion.hardware.HyperionMotor
import ca.helios5009.hyperion.hardware.Otos
import ca.helios5009.hyperion.misc.events.EventListener
import ca.helios5009.hyperion.pathing.PathBuilder
import ca.helios5009.hyperion.pathing.Point
import com.bylazar.battery.PanelsBattery
import com.bylazar.configurables.annotations.Configurable
import com.qualcomm.hardware.limelightvision.Limelight3A
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
import com.qualcomm.robotcore.hardware.AnalogInput
import com.qualcomm.robotcore.hardware.CRServoImplEx
import com.qualcomm.robotcore.hardware.HardwareMap
import com.qualcomm.robotcore.hardware.Servo
import com.qualcomm.robotcore.hardware.configuration.EthernetOverUsbConfiguration
import org.firstinspires.ftc.onbotjava.OnBotJavaWebInterfaceManager.instance
import org.firstinspires.ftc.teamcode.components.ColorSensor
import org.firstinspires.ftc.teamcode.components.Java_Beambreak
import org.firstinspires.ftc.teamcode.components.Java_Color_Sensor
import org.firstinspires.ftc.teamcode.components.Limelight
import org.firstinspires.ftc.teamcode.components.Selector
import org.firstinspires.ftc.teamcode.components.Shooter_Controller
import org.firstinspires.ftc.teamcode.components.Sorter
import org.firstinspires.ftc.teamcode.components.Turret
import kotlin.text.get

//import org.firstinspires.ftc.teamcode.components.Sorter
//import org.firstinspires.ftc.teamcode.components.Turret

class Robot(private val instance: LinearOpMode, events:EventListener, val alliance: Selector.alliance, is_auto:Boolean = false) {
    // Intake
    val intake = HyperionMotor(instance.hardwareMap, "CH")
    val transfer = HyperionMotor(instance.hardwareMap, "Belt")
    val gl = instance.hardwareMap.get(Servo::class.java, "GL")
    val gr = instance.hardwareMap.get(Servo::class.java, "GR")
    val gm = instance.hardwareMap.get(Servo::class.java, "GM")
    val csr = Java_Color_Sensor().get("CSR", instance.hardwareMap)
    val csm = Java_Color_Sensor().get("CSM", instance.hardwareMap)
    val csl = Java_Color_Sensor().get("CSL", instance.hardwareMap)
    // CLASSES
    val CS = ColorSensor(this)
    val SORT = Sorter(this)


    // Turret
    val fly_top = HyperionMotor(instance.hardwareMap, "TOP")
    val fly_bottom = HyperionMotor(instance.hardwareMap, "BOTTOM")

    val hood = instance.hardwareMap.get(Servo::class.java, "Hood")
    val left_spin = instance.hardwareMap.get(CRServoImplEx::class.java, "LS")
    val right_spin = instance.hardwareMap.get(CRServoImplEx::class.java, "RS")
    val turret_en = instance.hardwareMap.get(AnalogInput::class.java, "turret_en")
    // Limelight 3A
    val ll = instance.hardwareMap.get(Limelight3A::class.java, "LL")
    // Classes
    val LL = Limelight(this, alliance)
    val T = Turret(this, alliance)

    val gamepad2 = instance.gamepad2

    // Pathing
    val motors = Motors(instance.hardwareMap, "FL", "FR", "BL", "BR")
    // Classes
    val otos = Otos(instance.hardwareMap, "OTOS", OtosConstant.offset)
    val path = PathBuilder(instance, events, motors, otos, true)
    fun telemetry(string: String, value: Any){
        instance.telemetry.addData(string, value)
    }
    init{
        if(is_auto){
            path.setDriveConstants(
                DriveConstants.GainSpeed,
                0.0,
                DriveConstants.kD,
                DriveConstants.Tolerance,
                Double.POSITIVE_INFINITY
            )
            path.setStrafeConstants(
                DriveConstants.GainSpeed,
                0.0,
                DriveConstants.kD,
                StrafeConstants.Tolerance,
                Double.POSITIVE_INFINITY
            )
            path.setRotateConstants(
                RotateConstants.GainSpeed,
                0.0,
                RotateConstants.kD,
                0.0,
                RotateConstants.Tolerance,
                Double.POSITIVE_INFINITY
            )
            path.setDistanceTolerance(2.0)
        }
    }
    fun autoStart(){
        T.pre_sort(2)
        T.set_sequence(LL.detectO())
        intake.power = 0.0
        gm.position = 0.45
        telemetry("order", T.order.toString())
    }
//    fun gameEnd(){
//        shooter.ll.stop()
//    }
}
fun readVoltage(): Double {
    val provider = PanelsBattery.provider
    val voltage = provider.batteryVoltage
    return voltage
}
@Configurable
object DriveConstants {
    @JvmField var GainSpeed = 0.05
    @JvmField var AccelerationLimit = 0.7
    @JvmField var kD = 0.006
    @JvmField var Tolerance = 3.0
    @JvmField var Deadband = 0.25
}
@Configurable
object StrafeConstants {
    @JvmField var GainSpeed = 0.096
    @JvmField var AccelerationLimit = 0.82
    @JvmField var kD = 0.0
    @JvmField var Tolerance = 3.0
    @JvmField var Deadband = 0.25
}
@Configurable
object RotateConstants {
    @JvmField var GainSpeed = 1.8
    @JvmField var AccelerationLimit = 0.5
    @JvmField var kD = 0.0
    @JvmField var Tolerance = Math.PI/24
    @JvmField var Deadband = Math.PI/48
}
@Configurable
object TurretPID {
    @JvmField var kP = 0.017
    @JvmField var kI = 0.0048
    @JvmField var kD = 0.0002
    @JvmField var kF = 0.0
}
@Configurable
object OtosConstant {
    @JvmField var offset = Point(0.0, 0.0)
}