copy of code from 2017

Author: lu0079

README.md

@@ -1,2 +1,6 @@
-# 2018
-3299's code for the 2018 season
+# Steamworks
+3299's code for the 2017 season. Programmed in Python with [RobotPy](https://robotpy.github.io).
+
+## Notable features
+- A Raspberry Pi on the robot does vision tracking with OpenCV and [GRIP](https://github.com/WPIRoboticsProjects/GRIP). It then publishes the results on Network Tables for the robot to use.
+- Organized in a convoluted system that separates logic, actuators, and other components. It uses neither MagicBot or the Command framework.  Don't ask.

__init__.py

@@ -0,0 +1,54 @@
+"""
+Runs the auto routine. Called once.
+"""
+from networktables import NetworkTable
+import time
+import wpilib
+
+class Autonomous(object):
+    def __init__(self, drive, randy, frontGear, backGear, vision):
+        self.drive     = drive
+        self.randy     = randy
+        self.frontGear = frontGear
+        self.backGear  = backGear
+        self.vision    = vision
+        self.sd        = NetworkTable.getTable('SmartDashboard')
+
+    def run(self):
+        self.randy.run(True) # deploy Randy
+
+        if (self.sd.getBoolean('autoAngle') == True):
+            # Drive forward
+            startTime = time.clock()
+            while (time.clock() - startTime < 2.3):
+                self.drive.driveToAngle(-0.65, 0, True)
+
+            # Stop
+            self.drive.cartesian(0, 0, 0)
+
+            # Turn 60 or -60 degrees
+            if (self.sd.getBoolean('isLeft') == True):
+                self.drive.driveToAngle(0, -60, False)
+            else:
+                self.drive.driveToAngle(0, 60, False)
+
+            # Do vision
+            if (self.vision.alignToPeg(direction=1) == False): # if returns an error, stop auto
+                return False
+
+            # Stop
+            self.drive.cartesian(0, 0, 0)
+
+        '''
+        Center peg
+        '''
+        startTime = time.clock()
+        while (time.clock() - startTime < 5.2):
+            self.drive.cartesian(0, -0.3, 0.025)
+        self.drive.cartesian(0, 0, 0)
+
+        # Activate front gear
+        self.frontGear.run(True)
+
+        # Stop Randy
+        self.randy.run(False)

autonomous.py

@@ -0,0 +1,17 @@
+'''
+Launches the camera streamers in a seperate
+thread to prevent the main robot thread
+from being overloaded.
+'''
+
+from cscore import CameraServer
+
+def main():
+    cs = CameraServer.getInstance()
+    cs.enableLogging()
+
+    usb1 = cs.startAutomaticCapture(dev=0)
+
+    usb1.setResolution(320, 240)
+
+    cs.waitForever()

cameras.py

@@ -0,0 +1,88 @@
+"""
+Provides various helper functions.
+"""
+
+import math
+
+def remap( x, oMin, oMax, nMin, nMax ): # thanks stackoverflow.com/a/15537393
+    #range check
+    if oMin == oMax:
+        print("Warning: Zero input range")
+        return None
+
+    if nMin == nMax:
+        print("Warning: Zero output range")
+        return None
+
+    #check reversed input range
+    reverseInput = False
+    oldMin = min( oMin, oMax )
+    oldMax = max( oMin, oMax )
+    if not oldMin == oMin:
+        reverseInput = True
+
+    #check reversed output range
+    reverseOutput = False
+    newMin = min( nMin, nMax )
+    newMax = max( nMin, nMax )
+    if not newMin == nMin :
+        reverseOutput = True
+
+    portion = (x-oldMin)*(newMax-newMin)/(oldMax-oldMin)
+    if reverseInput:
+        portion = (oldMax-x)*(newMax-newMin)/(oldMax-oldMin)
+
+    result = portion + newMin
+    if reverseOutput:
+        result = newMax - portion
+
+    return result
+
+'''
+Given a value and power, this raises the value
+to the power, then negates it if the value was
+originally negative.
+'''
+
+def raiseKeepSign(value, power):
+    newValue = value ** power
+
+    if (value < 0 and newValue > 0):
+        return newValue * -1
+    else:
+        return value ** power
+
+'''
+Given a Cartesian x,y position, this 'snaps'
+it to the nearest angle, in degrees (the
+number of snappable angles is determined by
+`divisions`). Intended to be used with joystick
+values.
+'''
+def snap(divisions, x, y):
+    if (x == 0):
+        return 0
+
+    result = round(math.atan2(y, x) / (2 * math.pi / divisions) + divisions, 0) % divisions
+
+    return result * (360 / divisions)
+
+'''
+Maps a value onto a sin curve. Made for
+the driving fuctions.
+'''
+def curve(value):
+    return math.sin(value ** 2) * math.pi/2.6
+
+'''
+Rotates a vector in Caresian space.
+'''
+def rotateVector(x, y, angle):
+    angle = math.radians(angle)
+    cosA = math.cos(angle)
+    sinA = math.sin(angle)
+    return (x * cosA - y * sinA), (x * sinA + y * cosA)
+
+def normalizeAngle(angle):
+    '''Normalize angle to [-180,180]'''
+    return ((angle + 180) % 360) - 180.0

helpers.py

@@ -0,0 +1,36 @@
+"""
+Inits wpilib objects
+"""
+
+import wpilib
+from map import Map
+
+class Component(object):
+    def __init__(self):
+        # Mapping object stores port numbers for all connected motors, sensors, and joysticks. See map.py.
+        Mapping = Map()
+
+        # 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 other motors
+        self.shooterM    = wpilib.Spark(Mapping.shooterM)
+        self.hopperM     = wpilib.Spark(Mapping.hopperM)
+        self.climbM      = wpilib.Spark(Mapping.climbM)
+        self.groundGearM = wpilib.Talon(Mapping.groundGearM)
+
+        # Init soleniods
+        self.gearSol    = wpilib.DoubleSolenoid(Mapping.gearSol['out'], Mapping.gearSol['in'])
+        self.groundSol  = wpilib.DoubleSolenoid(Mapping.groundGearSol['out'], Mapping.groundGearSol['in'])
+
+        # Init sensors
+        self.gyroS      = wpilib.ADXRS450_Gyro(Mapping.gyroS)
+        self.allienceS  = wpilib.DigitalInput(Mapping.allienceS)
+        self.shooterS   = wpilib.DigitalInput(Mapping.shooterS)
+        self.hopperS    = wpilib.DigitalInput(Mapping.hopperS)
+
+        # Init relays
+        self.bumpPopR   = wpilib.Relay(Mapping.bumpPopR)
+        self.greenLEDR  = wpilib.Relay(Mapping.greenLEDR)

inits.py

@@ -0,0 +1,31 @@
+"""
+Defines port numbers for motors and sensors.
+"""
+
+class Map(object):
+    def __init__(self):
+        # Motors have suffix 'M'. All motors use PWM.
+        self.frontLeftM  = 3
+        self.frontRightM = 1
+        self.backLeftM   = 7
+        self.backRightM  = 0
+
+        self.climbM      = 2
+        self.collectorM  = 5
+        self.shooterM    = 9
+        self.hopperM     = 8
+        self.groundGearM = 4
+
+        # Soleniods
+        self.gearSol     = {'in': 3, 'out': 4}
+        self.groundGearSol = {'in': 2, 'out': 0}
+
+        # Sensors have suffix 'S'. Gyro uses SPI, everything else uses the DIO.
+        self.gyroS       = 0
+        self.allienceS   = 0
+        self.shooterS    = 1
+        self.hopperS     = 2
+
+        # Relays
+        self.bumpPopR    = 0
+        self.greenLEDR   = 1

map.py

@@ -0,0 +1,130 @@
+"""
+Main logic code
+"""
+import wpilib
+import time
+from networktables import NetworkTable
+
+from inits import Component
+import helpers
+
+from components.chassis import Chassis
+from components.gear import GearSol
+from components.climber import Climber
+from components.bumpPop import BumpPop
+from components.shooter import Shooter
+from components.groundGear import GroundGear
+
+from components.vision import Vision
+from autonomous import Autonomous
+
+class Randy(wpilib.SampleRobot):
+    def robotInit(self):
+        # init cameras
+        wpilib.CameraServer.launch('cameras.py:main')
+
+        self.C = Component() # Components inits all connected motors, sensors, and joysticks. See inits.py.
+
+        # Setup subsystems
+        self.drive             = Chassis(self.C.driveTrain, self.C.gyroS)
+        self.climb             = Climber(self.C.climbM)
+        self.bumpPop           = BumpPop(self.C.bumpPopR)
+        self.groundGear        = GroundGear(self.C.groundSol, self.C.groundGearM)
+        self.gearSol           = GearSol(self.C.gearSol)
+        self.shooter           = Shooter(self.C.shooterM, self.C.hopperM, self.C.shooterS, self.C.hopperS)
+        self.vision            = Vision(self.drive, self.C.greenLEDR)
+        self.autonomousRoutine = Autonomous(self.drive, self.bumpPop, self.gearSol, self.groundGear, self.vision)
+
+        # Smart Dashboard
+        self.sd = NetworkTable.getTable('SmartDashboard')
+        self.sd.putBoolean('autoAngle', False)
+        self.sd.putBoolean('isLeft', False)
+
+        # Joysticks or xBox controller?
+        self.controller = 'joysticks' # || joysticks
+
+        if (self.controller == 'joysticks'):
+            self.C.leftJ = wpilib.Joystick(0)
+            self.C.middleJ = wpilib.Joystick(1)
+            self.C.rightJ = wpilib.Joystick(2)
+        elif (self.controller == 'xbox'):
+            self.C.joystick = wpilib.XboxController(0)
+
+    def operatorControl(self):
+        # runs when robot is enabled
+        while self.isOperatorControl() and self.isEnabled():
+            '''
+            Components
+            '''
+            # Drive
+            if (self.controller == 'joysticks'):
+                self.drive.run(self.C.leftJ.getX(),
+                               self.C.leftJ.getY(),
+                               self.C.middleJ.getX(),
+                               self.C.leftJ.getRawButton(4),
+                               self.C.leftJ.getRawButton(3),
+                               self.C.leftJ.getRawButton(5),
+                               self.C.leftJ.getRawButton(2))
+
+            elif (self.controller == 'xbox'):
+                self.drive.arcade(self.C.joystick.getRawAxis(0), self.C.joystick.getRawAxis(1), self.C.joystick.getRawAxis(4))
+
+            # Back gear
+            if (self.controller == 'joysticks'):
+                if (self.C.middleJ.getRawButton(4)):
+                    self.groundGear.run(True, 'out')
+                elif (self.C.middleJ.getRawButton(5)):
+                    self.groundGear.run(True, 'in')
+                else:
+                    self.groundGear.run(False, False)
+
+            elif (self.controller == 'xbox'):
+                if (self.C.joystick.getBumper(wpilib.GenericHID.Hand.kLeft)):
+                    self.groundGear.run(True, 'out')
+                elif (self.C.joystick.getTriggerAxis(wpilib.GenericHID.Hand.kLeft) > 0.5):
+                    self.groundGear.run(True, 'in')
+                else:
+                    self.groundGear.run(False, False)
+
+            # Front gear
+            if (self.controller == 'joysticks'):
+                if (self.C.middleJ.getRawButton(1) == True and self.C.middleJ.getRawButton(2) == True):
+                    self.gearSol.run(True)
+                else:
+                    self.gearSol.run(False)
+
+            elif (self.controller == 'xbox'):
+                if (self.C.joystick.getTriggerAxis(wpilib.GenericHID.Hand.kRight) > 0.5):
+                    self.gearSol.run(True)
+                else:
+                    self.gearSol.run(False)
+
+            # LEDs
+            if (self.controller == 'xbox'):
+                if (self.C.joystick.getStickButton(wpilib.GenericHID.Hand.kRight)):
+                    self.C.greenLEDR.set(wpilib.Relay.Value.kForward)
+                else:
+                    self.C.greenLEDR.set(wpilib.Relay.Value.kOff)
+            elif (self.controller == 'joysticks'):
+                if (self.C.middleJ.getRawButton(3)):
+                    self.C.greenLEDR.set(wpilib.Relay.Value.kForward)
+                else:
+                    self.C.greenLEDR.set(wpilib.Relay.Value.kOff)
+
+            # Climb
+            if (self.controller == 'joysticks'):
+                self.climb.run(self.C.rightJ.getRawButton(1))
+            elif (self.controller == 'xbox'):
+                self.climb.run(self.C.joystick.getBumper(wpilib.GenericHID.Hand.kRight))
+
+            wpilib.Timer.delay(0.002) # wait for a motor update time
+
+    def test(self):
+        """This function is called periodically during test mode."""
+
+    def autonomous(self):
+        """Runs once during autonomous."""
+        self.autonomousRoutine.run() # see autonomous.py
+
+if __name__ == "__main__":
+    wpilib.run(Randy)