Organized-Code/drivetrain.py at master · RavenRoboticsTeam1288/Organized-Code · 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
#Raven Robotics 2019 Deep Space
#
#This class implements control for the robot's driving mechanism,
#apecifically a mecanum drive.
#

import wpilib
from wpilib.drive import MecanumDrive

class DrivetrainController():

    def __init__(self, robot):

        #Drivetrain Motor Setup
        self.rightFrontMotor = wpilib.Talon(7) #Right front
        self.rightBackMotor = wpilib.Talon(6) #Right back
        self.leftFrontMotor = wpilib.Talon(8) #Left front
        self.leftBackMotor = wpilib.Talon(9) #Left back

        #Mecanum Drive Setup
        robot.drive = MecanumDrive(self.leftFrontMotor,
                                  self.leftBackMotor,
                                  self.rightFrontMotor,
                                  self.rightBackMotor)


    def teleopPeriodic(self, robot):

        #Joystick Axis Setup
        stick1_X = robot.stick1.getX()
        stick2_Y = robot.stick2.getY()
        stick2_X = robot.stick2.getX()

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

        #Mecanum Drive Setup
        robot.drive.driveCartesian( -stick2_X, -stick2_Y, -stick1_X, 0 )

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

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