Lots of work. Week 0 tomorrow!

.gitignore

@@ -1,3 +1,5 @@
+tests/.pytest_cache
+
 # Byte-compiled / optimized / DLL files
 __pycache__/
 *.py[cod]

components/chassis.py

@@ -13,12 +13,13 @@ def __init__(self, drive, gyro):
         self.drive          = drive
         self.gyro           = gyro
         self.jDeadband      = 0.05
+
+        self.usePID         = False
         self.pidAngle       = wpilib.PIDController(0.03, 0, 0.1, self.gyro, output=self)
         self.sd             = NetworkTables.getTable('SmartDashboard')
         self.sd.putNumber('P', 0.022)
         self.sd.putNumber('I', 0)
         self.sd.putNumber('D', 0)
-
         self.pidAngle.setInputRange(-180.0, 180.0)
         self.pidAngle.setOutputRange(-1.0, 1.0)
         self.pidAngle.setAbsoluteTolerance(5)
@@ -38,23 +39,24 @@ def arcade(self, x1, y1, x2):
 
     def cartesian(self, x, y, rotation):
         """Uses the gryo to compensate for bad design :P"""
-        self.pidAngle.setP(self.sd.getNumber("P", 0.03))
-        self.pidAngle.setI(self.sd.getNumber("I", 0))
-        self.pidAngle.setD(self.sd.getNumber("D", 0.1))
-        if rotation == 0:
-            # reset gryo when rotation stops
-            if (self.wasRotating):
-                self.gyro.reset()
-                self.wasRotating = False
-
-            # PID controller
-            self.pidAngle.setSetpoint(0)
-            self.pidAngle.enable()
-            self.pidAngle.setContinuous(True)
-            rotation = -self.pidRotateRate
-        else:
-            # if there's non-zero rotation input from the joystick, don't run the PID loop
-            self.wasRotating = True
+        if (self.usePID != False):
+            self.pidAngle.setP(self.sd.getNumber("P", 0.03))
+            self.pidAngle.setI(self.sd.getNumber("I", 0))
+            self.pidAngle.setD(self.sd.getNumber("D", 0.1))
+            if rotation == 0:
+                # reset gryo when rotation stops
+                if (self.wasRotating):
+                    self.gyro.reset()
+                    self.wasRotating = False
+
+                # PID controller
+                self.pidAngle.setSetpoint(0)
+                self.pidAngle.enable()
+                self.pidAngle.setContinuous(True)
+                rotation = -self.pidRotateRate
+            else:
+                # if there's non-zero rotation input from the joystick, don't run the PID loop
+                self.wasRotating = True
 
 
         # assign speeds
@@ -64,12 +66,19 @@ def cartesian(self, x, y, rotation):
         speeds[2] = -x - y - rotation
         speeds[3] = -x + y - rotation
 
-        # TODO: this will currrently scale speeds if over `1`, but not if under `-1`
-        if (max(speeds) > 1):
-            maxSpeed = max(speeds)
+        # scales all speeds if one is in range
+        # (-inf, -1) U (1, inf)
+        maxSpeed = max(speeds)
+        minSpeed = min(speeds)
+
+        if (maxSpeed > 1):
             for i in range (0, 4):
                 if (speeds[i] != 0):
-                    speeds[i] = maxSpeed / speeds[i]
+                    speeds[i] = speeds[i] / maxSpeed
+        elif (minSpeed < -1):
+            for i in range(0, 4):
+                if (speeds[i] != 0):
+                    speeds[i] =  - (speeds[i] / minSpeed)
 
         # set scaled speeds
         self.drive['frontLeft'].set(self.curve(speeds[0]))

components/lights.py

@@ -9,7 +9,7 @@ class Lights(object):
     def __init__(self):
         # Init I2C for communication with Arduino
         if (hal.isSimulation()):
-            # imports stub for simulation
+            # import stub for simulation
             from components.i2cstub import I2CStub
             self.arduinoC = wpilib.I2C(wpilib.I2C.Port.kOnboard, 4, I2CStub())
         else:

components/metabox.py

@@ -0,0 +1,15 @@
+"""
+Controls intake wheels, the elevator, and the metabox.
+"""
+
+class MetaBox(object):
+    def __init__(self, encoder, elevatorM):
+        self.encoder = encoder
+        self.elevatorM = elevatorM
+
+    def run(self, value):
+        self.elevatorM.set(value)
+        print(self.getEncoder())
+
+    def getEncoder(self):
+        return self.encoder.getDistance()

components/pdb.py

@@ -0,0 +1,16 @@
+"""
+Wrapper for wpilib.PowerDistributionPanel().
+"""
+import wpilib
+
+class Power(object):
+    def __init__(self):
+        self.board = wpilib.PowerDistributionPanel()
+
+    def getAverageCurrent(self, slots):
+        totalCurrent = 0
+
+        for slot in slots:
+            totalCurrent += self.board.getCurrent(slot)
+
+        return totalCurrent / len(slots)

inits.py

@@ -12,8 +12,11 @@ def __init__(self):
 
         # Init drivetrain
         self.driveTrain = {'frontLeft': wpilib.Spark(Mapping.frontLeftM), 'backLeft': wpilib.Spark(Mapping.backLeftM), 'frontRight': wpilib.Spark(Mapping.frontRightM), 'backRight': wpilib.Spark(Mapping.backRightM)}
-        #self.driveTrain['frontLeft'].setInverted(True)
-        #self.driveTrain['backLeft'].setInverted(True)
+
+        # Init motors
+        self.elevatorM = wpilib.Spark(Mapping.elevatorM)
 
         # Init sensors
-        self.gyroS = wpilib.ADXRS450_Gyro(Mapping.gyroS)
+        self.gyroS = wpilib.AnalogGyro(Mapping.gyroS)
+        self.elevatorS = wpilib.Encoder(7, 8)
+        self.elevatorS.setDistancePerPulse(0.064)

map.py

@@ -9,7 +9,8 @@ def __init__(self):
         self.frontRightM = 3
         self.backLeftM   = 1
         self.backRightM  = 5
+        self.elevatorM   = 2
 
-        # Sensors have suffix 'S'. Gyro uses SPI, everything else uses the DIO.
+        # Sensors have suffix 'S'. Gyro uses analog, everything else uses the DIO.
         self.gyroS = 0
         self.arduino = 9

robot.py

@@ -11,6 +11,8 @@
 
 from autonomous import Autonomous
 from components.lights import Lights
+from components.metabox import MetaBox
+from components.pdb import Power
 
 class Randy(wpilib.SampleRobot):
     def robotInit(self):
@@ -22,6 +24,8 @@ def robotInit(self):
         # Setup subsystems
         self.drive = Chassis(self.C.driveTrain, self.C.gyroS)
         self.lights = Lights()
+        self.metabox = MetaBox(self.C.elevatorS, self.C.elevatorM)
+        self.power = Power()
 
         self.autonomousRoutine = Autonomous(self.drive)
 
@@ -55,6 +59,8 @@ def operatorControl(self):
             elif (self.controller == 'xbox'):
                 self.drive.arcade(self.C.joystick.getRawAxis(0), self.C.joystick.getRawAxis(1), self.C.joystick.getRawAxis(4))
 
+            self.metabox.run(self.C.leftJ.getY())
+
             wpilib.Timer.delay(0.002) # wait for a motor update time
 
     def test(self):