Robot-Code-2020/drivetrain.py at master · RavenRoboticsTeam1288/Robot-Code-2020 · 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
#Raven Robotics 2020 Infinite Recharge
#
#This class implements control for the robot's driving mechanism,
#Specifically a tank drive.
#

import wpilib
from wpilib.drive import DifferentialDrive
from wpilib.drive import MecanumDrive
import ctre
import utilities
from utilities import utilities

class DrivetrainController():

    def __init__(self, robot):# This defines __init__(self, robot):

        #Drivetrain Motor Setup
        #Setup for Tank Drive
        self.frontLeft = ctre.WPI_TalonSRX(12)
        #self.frontLeft.setInverted(True)
        self.rearLeft = ctre.WPI_TalonSRX(2)
        self.left = wpilib.SpeedControllerGroup(self.frontLeft, self.rearLeft)
        self.frontRight = ctre.WPI_TalonSRX(8)
        #self.frontRight.setInverted(True)
        self.rearRight = ctre.WPI_TalonSRX(4)
        self.right = wpilib.SpeedControllerGroup(self.frontRight, self.rearRight)
        robot.drive = DifferentialDrive(self.left, self.right)

        #Can bus Mecanum Drive
        '''self.frontLeft = ctre.WPI_VictorSPX(4)
        self.rearLeft = ctre.WPI_VictorSPX(3)
        self.frontRight = wpilib.Talon(0)
        self.rearRight = ctre.WPI_VictorSPX(2)
        robot.drive = MecanumDrive(self.frontLeft, self.rearLeft, self.frontRight, self.rearRight)'''

    #---------------------------------------------------------------------------------------------------

    def teleopPeriodic(self, robot):

        #print("Teleop")
        #Joystick Axis Setup
        stick1_Y = robot.stick1.getY()
        stick2_Y = robot.stick2.getY()

        #Dominant Joystick thing
        if robot.stick1.getRawButton(1):
            stick2_Y = stick1_Y
        if robot.stick2.getRawButton(1):
            stick1_Y = stick2_Y
            #print("Getting axis")

        #Joystick Deadzone Setup
        if stick1_Y > -0.05 and stick1_Y < 0.05:
            stick1_Y = 0
        if stick2_Y > -0.05 and stick2_Y < 0.05:
            stick2_Y = 0

        #Fine range control
        if robot.stick1.getRawButton(2) or robot.stick2.getRawButton(2):
            stick1_Y *= 0.5
            stick2_Y *= 0.5

        if robot.gamepad.getRawButton(1):
            utilities.ControlPanelDriving(robot)

        #Tank Drive Setup
        robot.drive.tankDrive(-stick1_Y, -stick2_Y)

    #--------------------------------------------------------------------

    def autonomousInit(self, robot):
        robot.drive.setSafetyEnabled( False )
        pass

    def autonomousPeriodic(self, robot):
        autostate = 'Ideal'
    #Ideal Start™
        if autostate == 'Ideal':
            done = utilities.driveNumInches(robot, 1, .5, 24)
            if done:
                autostate = 'done'
        elif autostate == 'aim':
            done = utilities.ultrasonicAim(robot, 2)
            if done:
                autostate = 'shoot'
        elif autostate == 'shoot':
            pass #shooting auto code
        elif autostate == 'done': #says auto is complete, you can rest until teleop
            return True

    #First alternate Start™
        if autostate == 'Alt1':
            print("gotz to drivezz")
            pass #gyro
        elif autostate == 'move':
            done = utilities.DriveNumInches(MyRobot, 1, .5, 48)
            if done:
                autostate = 'aim'
        elif autostate == 'aim':
            done = utilities.ultrasonicAim(MyRobot, 'FIX HERE')
            if done:
                autostate = 'shoot'
        elif autostate == 'shoot':
            pass #shooting auto code
        elif autostate == 'done': #says auto is complete, you can rest until teleop
            return True

    #second alternate start tm
        if autostate == 'Alt2':
            print("gotz to drivez")
            pass #gyro
        elif autostate == 'move':
            done = utilities.DriveNumInches(MyRobot, 1, .5, (30 * 12))
            if done:
                autostate = 'aim'
        elif autostate == 'aim':
            done = utilities.ultrasonicAim(MyRobot, 'FIX HERE')
            if done:
                autostate = 'shoot'
        elif autostate == 'shoot':
            pass #shooting auto code
        elif autostate == 'done': #says auto is complete, you can rest until teleop
            return True


    def teleopInit(self, robot):
        robot.drive.setSafetyEnabled( True )
        pass