RobotCodePythonTank2026/robot.py at main · 5724-SpartanRobotics/RobotCodePythonTank2026 · 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
import wpilib
import wpilib.drive
import phoenix5


class MyRobot(wpilib.TimedRobot):

    def robotInit(self):
        """Robot initialization function"""

        #power distribution board
        self.pdp = wpilib.PowerDistribution(0, wpilib.PowerDistribution.ModuleType.kCTRE)

        #motor stuff
        self.leftMotor1 = phoenix5.WPI_VictorSPX(1)
        self.leftMotor2 = phoenix5.WPI_VictorSPX(2)
        self.rightMotor1 = phoenix5.WPI_VictorSPX(3)
        self.rightMotor2 = phoenix5.WPI_VictorSPX(4)

        self.leftMotor1.setNeutralMode(phoenix5.NeutralMode.Brake)
        self.leftMotor2.setNeutralMode(phoenix5.NeutralMode.Brake)
        self.rightMotor1.setNeutralMode(phoenix5.NeutralMode.Brake)
        self.rightMotor2.setNeutralMode(phoenix5.NeutralMode.Brake)

        self.leftMotor2.follow(self.leftMotor1)
        self.rightMotor2.follow(self.rightMotor1)

        self.robotDrive = wpilib.drive.DifferentialDrive(self.leftMotor1, self.rightMotor1)
        self.driverController = wpilib.XboxController(0)

        # We need to invert one side of the drivetrain so that positive voltages
        # result in both sides moving forward. Depending on how your robot's
        # gearbox is constructed, you might have to invert the left side instead.
        self.rightMotor1.setInverted(True)


    def teleopPeriodic(self):
        # Drive with split arcade style
        # That means that the Y axis of the left stick moves forward
        # and backward, and the X of the right stick turns left and right.
        self.robotDrive.arcadeDrive(
            -self.driverController.getLeftY(), -self.driverController.getRightX()
        )


        # add pdp displays in to smart dashboard
        # Get the total current of all channels.
        totalCurrent = self.pdp.getTotalCurrent()
        wpilib.SmartDashboard.putNumber("Total Current", totalCurrent)

        # Get the total power of all channels.
        totalPower = self.pdp.getTotalPower()
        wpilib.SmartDashboard.putNumber("Total Power", totalPower)

        # Get the total energy of all channels.
        totalEnergy = self.pdp.getTotalEnergy()
        wpilib.SmartDashboard.putNumber("Total Energy", totalEnergy)