straight line

.deploy_cfg

@@ -0,0 +1,3 @@
+[auth]
+hostname = roborio-3299-frc.local
+

autonomous.py

@@ -1,7 +1,6 @@
 """
 Runs the auto routine. Called once.
 """
-from networktables import NetworkTable
 import time
 import wpilib
 

components/bumpPop.py

@@ -0,0 +1,14 @@
+"""
+Pushes a bar out in front of the bumpers for aligning with the gear peg.
+"""
+import wpilib
+
+class BumpPop(object):
+    def __init__(self, output):
+        self.output = output
+
+    def run(self, trigger):
+        if (trigger == True):
+            self.output.set(wpilib.Relay.Value.kForward)
+        else:
+            self.output.set(wpilib.Relay.Value.kOff)

components/chassis.py

@@ -0,0 +1,117 @@
+"""
+Drives. Can accept input from joysticks or values [-1, 1].
+Uses the wheel-attached encoders as input for a threaded PID
+loop on each wheel.
+"""
+import helpers
+import wpilib
+import math
+
+
+class Chassis(object):
+    def __init__(self, drive, gyro):
+        self.drive          = drive
+        self.gyro           = gyro
+        self.jDeadband      = 0.05
+        self.pidAngle       = wpilib.PIDController(0.03, 0, 0.1, self.gyro, output=self)
+
+        self.pidAngle.setInputRange(-180.0, 180.0)
+        self.pidAngle.setOutputRange(-1.0, 1.0)
+        self.pidAngle.setAbsoluteTolerance(5)
+        self.pidAngle.setContinuous(False)
+        self.pidRotateRate = 0
+
+    def run(self, leftX, leftY, rightX, microLeft, microTop, microRight, microBackward):
+        self.arcade(helpers.raiseKeepSign(leftX, 2) + 0.4*(microRight - microLeft),
+                    helpers.raiseKeepSign(leftY, 2) + 0.4*(microBackward - microTop),
+                    rightX)
+
+    def arcade(self, x1, y1, x2):
+        # rotation curve
+        rotation = helpers.raiseKeepSign(-x2, 2) * 0.75
+        self.cartesian(x1, y1, rotation)
+
+    def cartesian(self, x, y, rotation):
+        speeds = [0] * 4
+        angle = self.gyro.getAngle()
+
+
+        #self.gyro.reset()
+        if rotation == 0:
+            self.pidAngle.setSetpoint(0)
+            self.pidAngle.enable()
+            self.pidAngle.setContinuous(True)
+            rotation = -self.pidRotateRate
+        elif self.pidAngle.onTarget():
+            self.gyro.reset()
+
+        #rotation= helpers.remap(self.pidRotateRate, -180, 180, -1, 1)
+        print(rotation)
+
+        speeds[0] = -x + y + rotation
+        speeds[1] = x + y - rotation
+        speeds[2] = x + y + rotation
+        speeds[3] = -x + y - rotation
+
+        if (max(speeds) > 1):
+            maxSpeed = max(speeds)
+            for i in range (0, 4):
+                if (speeds[i] != 0):
+                    speeds[i] = maxSpeed / speeds[i]
+
+        self.drive['frontLeft'].set(speeds[0])
+        self.drive['frontRight'].set(speeds[1])
+        self.drive['backLeft'].set(speeds[2])
+        self.drive['backRight'].set(speeds[3])
+
+    def polar(self, power, direction, rotation):
+        power = power * math.sqrt(2.0)
+        # The rollers are at 45 degree angles.
+        dirInRad = math.radians(direction + 45)
+        cosD = math.cos(dirInRad)
+        sinD = math.sin(dirInRad)
+
+        speeds = [0] * 4
+        speeds[0] = sinD * power + rotation
+        speeds[1] = cosD * power - rotation
+        speeds[2] = cosD * power + rotation
+        speeds[3] = sinD * power - rotation
+
+        self.drive['frontLeft'].set(speeds[0])
+        self.drive['frontRight'].set(speeds[1])
+        self.drive['backLeft'].set(speeds[2])
+        self.drive['backRight'].set(speeds[3])
+
+    def driveToAngle(self, power, angle, continuous):
+        self.gyro.reset()
+        self.pidAngle.setSetpoint(angle)
+        self.pidAngle.enable()
+        self.pidAngle.setContinuous(continuous)
+
+        if (continuous == True): # if true, runs continuously (for driving straight)
+            print(self.pidRotateRate)
+            self.cartesian(0, -power, -self.pidRotateRate)
+        else:
+            while (abs(self.pidAngle.getError()) > 2):
+                print(self.pidAngle.getError())
+                self.cartesian(0, 0, -self.pidRotateRate)
+
+            self.pidAngle.disable()
+            self.cartesian(0, 0, 0)
+            self.gyro.reset()
+            return;
+
+    def anglePIDInput(self):
+        return self.gyro.getAngle()
+
+    def pidWrite(self, value):
+        self.pidRotateRate = value
+
+    def speedsToDirection(self, frontLeft, frontRight, backLeft, backRight):
+    	x = (backLeft - frontLeft) / 2
+    	y = (frontLeft + frontRight) / 2
+    	rotation = (backLeft - frontRight) / 2
+
+    	direction = math.degrees(math.atan2(x, -y))
+
+    	return {'direction': -direction, 'rotation': rotation}

components/climber.py

@@ -0,0 +1,13 @@
+"""
+Runs the climber motor.
+"""
+
+class Climber(object):
+    def __init__(self, motor):
+        self.motor = motor
+
+    def run(self, trigger):
+        if (trigger == True):
+            self.motor.set(-1)
+        else:
+            self.motor.set(0)

components/gear.py

@@ -0,0 +1,13 @@
+"""
+Pops the gear onto the peg. Uses solenoids.
+"""
+
+class GearSol(object):
+    def __init__(self, sol):
+        self.sol = sol
+
+    def run(self, trigger):
+        if (trigger == True):
+            self.sol.set(self.sol.Value.kForward)
+        else:
+            self.sol.set(self.sol.Value.kReverse)

components/groundGear.py

@@ -0,0 +1,23 @@
+"""
+Pops the gear onto the peg (from ground pickup). Uses solenoids.
+"""
+
+import wpilib
+
+class GroundGear(object):
+    def __init__(self, sol, rollers):
+        self.sol = sol
+        self.rollers = rollers
+
+    def run(self, trigger, rollerTrigger):
+        if (trigger == True):
+            self.sol.set(self.sol.Value.kForward)
+        else:
+            self.sol.set(self.sol.Value.kReverse)
+
+        if (rollerTrigger == 'in'):
+            self.rollers.set(1)
+        elif (rollerTrigger == 'out'):
+            self.rollers.set(-1)
+        else:
+            self.rollers.set(0)

components/shooter.py

@@ -0,0 +1,76 @@
+"""
+Shoots balls using cardboard and
+paper plates, tastefully decorated
+with EDM artists.
+"""
+
+class Shooter(object):
+    def __init__(self, shooterM, hopperM, shooterS, hopperS):
+        self.shooterM = shooterM
+        self.hopperM  = hopperM
+        self.shooterS = shooterS
+        self.hopperS  = hopperS
+
+        self.hopperState = 'off'
+        self.shooterState = 'off'
+        self.hopperDirection = 1
+        self.shooterDirection = -1
+
+        self.limitSwitchTriggered = True
+
+    def run(self, trigger):
+        if (trigger == True):
+            '''
+            Shooter
+            '''
+            if (self.shooterState == 'off' and self.hopperState == 'off'):
+                self.shooterState = 'running'
+            elif (self.shooterState == 'running' or self.shooterState == 'pastlimitswitch'):
+                # record when limit switch is no longer triggered
+                if (self.shooterS.get() != self.limitSwitchTriggered):
+                    self.shooterState == 'pastlimitswitch'
+
+                # limit switch is triggered, stop the motor
+                if (self.shooterS.get() == self.limitSwitchTriggered and self.shooterState == 'pastlimitswitch'):
+                    self.shooterState = 'finished'
+                else:
+                    self.shooterM.set(self.shooterDirection) # otherwise keep running the motor
+
+            # if 1 rev has completed, stop motor
+            elif (self.shooterState == 'finished'):
+                self.shooterState = 'off'
+                self.hopperState = 'running'
+
+            elif (self.shooterState == 'off'):
+                self.shooterM.set(0)
+
+            '''
+            Hopper
+            '''
+            # inital turn on
+            if (self.hopperState == 'off'):
+                self.hopperM.set(0)
+
+            # runs while plate makes 1 rev
+            elif (self.hopperState == 'running' or self.hopperState == 'pastlimitswitch'):
+                # record when limit switch is no longer triggered
+                if (self.hopperS.get() != self.limitSwitchTriggered):
+                    self.hopperState == 'pastlimitswitch'
+
+                # limit switch is triggered, stop the motor
+                if (self.hopperS.get() == self.limitSwitchTriggered and self.hopperState == 'pastlimitswitch'):
+                    self.hopperState = 'finished'
+                else:
+                    self.hopperM.set(self.hopperDirection) # otherwise keep running the motor
+
+            # if 1 rev has completed, stop the motor
+            elif (self.hopperState == 'finished'):
+                self.hopperM.set(0)
+
+                # ball is now in shooter
+                self.shooterState = 'running'
+
+
+        else:
+            self.hopperM.set(0)
+            self.shooterM.set(0)

components/sonic.py

@@ -0,0 +1,45 @@
+"""
+Helper class for ultrasonic sensor
+"""
+import statistics
+import wpilib
+
+class Sonic(object):
+    def __init__(self, sensor):
+        self.sensor = sensor
+        self.lastMeasurement = {'left': 0, 'right': 0}
+        self.lastSensor = 'left'
+        self.sensorOn = False
+        self.timer = wpilib.Timer()
+        self.lastTime = 0
+        self.period = 200
+
+    def run(self):
+        if ((self.timer.getMsClock() - self.lastTime) > self.period):
+            if (self.sensorOn == False):
+                if (self.lastSensor == 'left'):
+                    self.sensor['rightR'].pulse(255)
+                if (self.lastSensor == 'right'):
+                    self.sensor['leftR'].pulse(255)
+                self.sensorOn = True
+            else:
+                if (self.lastSensor == 'left'):
+                    averageVoltage = self.sensor['rightS'].getAverageVoltage()
+                    distance = (averageVoltage * 1000)/4.9
+                    self.sensor['rightR'].pulse(0)
+                    self.lastMeasurement['right'] = distance
+                    self.lastSensor = 'right'
+                if (self.lastSensor == 'right'):
+                    averageVoltage = self.sensor['leftS'].getAverageVoltage()
+                    distance = (averageVoltage * 1000)/4.9
+                    self.sensor['leftR'].pulse(0)
+                    self.lastMeasurement['left'] = distance
+                    self.lastSensor = 'left'
+
+                self.sensorOn = False
+
+            self.lastTime = self.timer.getMsClock()
+
+
+    def getCm(self, side):
+        return self.lastMeasurement[side]