2024-CrescendoCode/actuatorSystem.py at main · FathomRobotics/2024-CrescendoCode · 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
import wpilib
import commands2


# Note: Look here
# https://github.com/Lambda-Corps/2024_Crescendo/blob/main/intake.py

# Get the rotation of the robot from the gyro.
# Put Wrist Into Shooting Position
# self.wrist.set(-0.004 * self.wristPID.calculate(self.wristEncoder.get(), -4500))
# Start Shooter Motors
# self.shooter.set(0.005 * self.shooterPID.calculate(self.shooterEncoder.getVelocity(), 3500) + self.shooterVValueSub.get())
# self.shooterHelper.set(-0.005 * self.shooterHelperPID.calculate(self.shooterHelperEncoder.getVelocity(), 3500) - self.shooterHelperVValueSub.get())

# If Wrist in position start arm movement
# if self.wristEncoder.get() <= -4400:
#     self.autoArmDownStart = True

# If Arm in position after wrist is in position shoot
# if self.autoArmDownStart and (self.armBuiltinEncoder.getPosition() <= 30):
#     self.intake.set(0.5)  # Spit note out of jaws

# if self.autoArmDownStart:
#     self.arm.set(self.armPID.calculate(self.armBuiltinEncoder.getPosition(), 0))
# else:
#     self.arm.set(self.armPID.calculate(self.armBuiltinEncoder.getPosition(), 120))
class IntakeSystem(commands2.Subsystem):
    def __init__(self, intakeMotor, maxPower=0.5):
        super().__init__()
        self.intakeMaxPower = maxPower
        self.motor = intakeMotor
        self.noteDirection = "Neither"

    def noteOut(self):
        self.noteDirection = "Out"

    def noteIn(self):
        self.noteDirection = "In"

    def off(self):
        self.noteDirection = "Neither"

    def periodic(self):
        if self.noteDirection == "In":
            self.motor.set(-self.intakeMaxPower)
        elif self.noteDirection == "Out":
            self.motor.set(self.intakeMaxPower)
        else:
            self.motor.set(0)


class IntakeOff(commands2.InstantCommand):
    def __init__(self, intake: IntakeSystem):
        super().__init__()
        self._sub = intake
        self.addRequirements(self._sub)

    def initialize(self):
        pass

    def execute(self):
        self.intake.off()


class IntakeNote(commands2.InstantCommand):
    def __init__(self, intake: IntakeSystem):
        super().__init__()
        self._sub = intake
        self.addRequirements(self._sub)

    def initialize(self):
        pass

    def execute(self):
        self.intake.noteIn()


class IntakeSpitNote(commands2.InstantCommand):
    def __init__(self, intake: IntakeSystem):
        super().__init__()
        self._sub = intake
        self.addRequirements(self._sub)

    def initialize(self):
        pass

    def execute(self):
        self.intake.noteOut()