1786-2016-Robot-Code-Python/robot.py at master · Team1786/1786-2016-Robot-Code-Python · 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
#!/usr/bin/env python3
import wpilib
from enum import Enum
import math
import shooter

TRIGGER = 1
THUMB = 2
RAMP_RAISE = 4
RAMP_LOWER = 3
UNJAM = 11

class MyRobot(wpilib.IterativeRobot):
    def robotInit(self):
        """
		This function is called upon program startup and
		should be used for any initialization code.
		"""
        self.left_drive = wpilib.TalonSRX(0)
        self.right_drive = wpilib.TalonSRX(1)
        self.drive = wpilib.RobotDrive(self.left_drive, self.right_drive)

        self.shooter1 = wpilib.CANTalon(11)
        self.shooter2 = wpilib.CANTalon(10)
        self.ramp = wpilib.CANTalon(12)
        self.shooter = shooter.shooter(self.shooter1, self.shooter2, self.ramp)

        self.driver_stick = wpilib.Joystick(0)
        self.operator_stick = wpilib.Joystick(1)

        self.shooter1.enable()
        self.shooter2.enable()

        self.left_drive.setInverted(True)
        self.right_drive.setInverted(True)

        self.inverting = False
        self.pickupRunning = False
        self.ramping = False
        self.shooting = False
        self.unjamming = False
        self.arming = False
        self.shooterPower = 0
        self.arcade = False

    def autonomousInit(self):
        """This function is run once each time the robot enters autonomous mode."""
        self.auto_loop_counter = 0

    def autonomousPeriodic(self):
        """This function is called periodically during autonomous."""

        # Check if we've completed 100 loops (approximately 2 seconds)
        if self.auto_loop_counter < 100:
            self.drive.drive(-0.01, 0)  # Drive forwards at one hundreth speed
            self.auto_loop_counter += 1
        else:
            self.drive.drive(0, 0)  # Stop robot

    def teleopPeriodic(self):
        """This function is called periodically during operator control."""
        #self.drive.arcadeDrive(-self.driver_stick.getY(), -self.driver_stick.getX() * 0.75)
        if(self.driver_stick.getRawButton(7)):
            self.arcade = True
        if(self.driver_stick.getRawButton(8)):
            self.arcade = False
        if (self.arcade):
            self.drive.arcadeDrive(self.driver_stick)
        else:
            if (self.driver_stick.getRawButton(THUMB)):
                left = self.driver_stick.getTwist()
                right = -self.driver_stick.getTwist()
                self.drive.tankDrive(left, right)
            else:
                self.updateDrive()
        if (not self.ramping and self.operator_stick.getRawButton(RAMP_RAISE)):
            self.shooter.raiseRamp()
            self.ramping = True
        elif (not self.ramping and self.operator_stick.getRawButton(RAMP_LOWER)):
            self.shooter.lowerRamp()
            self.ramping = True
        elif (self.ramping and not self.operator_stick.getRawButton(RAMP_LOWER) and not self.operator_stick.getRawButton(RAMP_RAISE)):
            self.shooter.stopRamp()
            self.ramping = False
        if (not self.unjamming and self.operator_stick.getRawButton(UNJAM)):
            self.unjamming = True
            self.shooter.unJam()
        elif (not self.unjamming and self.operator_stick.getRawButton(TRIGGER)):
            self.shooter.shootLow()
            self.unjamming = True
        elif (self.unjamming and not self.operator_stick.getRawButton(UNJAM) and not self.operator_stick.getRawButton(TRIGGER)):
            self.shooter.pickUp(False)
            self.unjamming = False
        #comment here
        if(self.operator_stick.getRawButton(THUMB) and not self.pickupRunning):
            #added True Bool to self.shooter.pickUp(True)
            self.shooter.pickUp(True)
            self.pickupRunning = True
        elif (not self.operator_stick.getRawButton(THUMB) and self.pickupRunning):
            self.shooter.pickUp(False)
            self.pickupRunning = False

        if (self.driver_stick.getRawButton(TRIGGER) and not self.inverting):
            print("re-re-inverting")
            self.left_drive.setInverted(not self.left_drive.getInverted())
            self.right_drive.setInverted(not self.right_drive.getInverted())
            self.inverting = True
        elif not (self.driver_stick.getRawButton(TRIGGER)):
            self.inverting = False

        self.opThrottle = self.saneThrottle(self.operator_stick.getThrottle())

        if (not self.pickupRunning and not self.unjamming):
            self.shooter.setPower(self.opThrottle)

    def testPeriodic(self):
        """This function is called periodically during test mode."""
        wpilib.LiveWindow.run()

    def saneThrottle(self, rawThrottle):
        return ((1.0 - rawThrottle) / 2.0)

    def updateDrive(self):
        x = -self.driver_stick.getX()
        y = -self.driver_stick.getY()
        if (x > 0):
            left = y * self.saneThrottle(self.driver_stick.getThrottle())
            right = (1 - x) * y * self.saneThrottle(self.driver_stick.getThrottle())
            self.drive.tankDrive(left, right)
        else:
            left = y * self.saneThrottle(self.driver_stick.getThrottle())
            right = (1 + x) * y * self.saneThrottle(self.driver_stick.getThrottle())
            self.drive.tankDrive(left, right)

if __name__ == "__main__":
    wpilib.run(MyRobot)