Add components to metabox and fixed toDistance(), toAngle()

components/chassis.py

@@ -16,7 +16,7 @@ def __init__(self, drive, gyro, encoderY):
         self.jDeadband      = 0.06
         self.sd             = NetworkTables.getTable('SmartDashboard')
 
-        # Boundry on speed (in / sec)
+        # Boundary on speed (in / sec)
         self.maxSpeed = 37.5
 
         # PID loop for angle
@@ -28,28 +28,26 @@ def __init__(self, drive, gyro, encoderY):
         self.sd.putNumber('pidAngleD', self.pidAngleDefault['d'])
 
         self.pidAngle = wpilib.PIDController(self.pidAngleDefault['p'], self.pidAngleDefault['i'], self.pidAngleDefault['d'], self.gyro, self.updateAnglePID)
-        self.pidAngle.setInputRange(-180.0, 180.0)
         self.pidAngle.setOutputRange(-1.0, 1.0)
-        self.pidAngle.setAbsoluteTolerance(5)
-        self.pidAngle.setContinuous(False)
+        self.pidAngle.setAbsoluteTolerance(2)
         self.pidRotateRate = 0
         self.wasRotating = False
 
         # PID loop for Cartesian Y direction
         self.useYPID = False
 
-        self.pidYDefault = {'p': 0.05, 'i': 0, 'd': 0.02}
+        self.pidYDefault = {'p': 0.08, 'i': 0, 'd': 0}
         self.sd.putNumber('pidYP', self.pidYDefault['p'])
         self.sd.putNumber('pidYI', self.pidYDefault['i'])
         self.sd.putNumber('pidYD', self.pidYDefault['d'])
 
-        self.pidY = wpilib.PIDController(self.pidYDefault['p'], self.pidYDefault['i'], self.pidYDefault['d'], self.encoderY.getRate, self.updateYPID)
-        self.pidY.setInputRange(-self.maxSpeed, self.maxSpeed)
-        self.pidY.setOutputRange(-1.0, 1.0)
-        self.pidY.setContinuous(True)
+        self.pidY = wpilib.PIDController(self.pidYDefault['p'], self.pidYDefault['i'], self.pidYDefault['d'], self.encoderY.getDistance, self.updateYPID)
         self.pidYRate = 0
 
-    def cartesian(self, x, y, rotation):
+        self.toDistanceFirstCall = True
+
+    def run(self, x, y, rotation):
+        '''Intended for use in telelop. Use .cartesian for auto.'''
         # Map joystick values to curve
         x = self.curve(x)
         y = self.curve(y)
@@ -67,21 +65,25 @@ def cartesian(self, x, y, rotation):
                     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.useYPID != False):
-            mappedY = helpers.remap(y, -1, 1, -self.maxSpeed, self.maxSpeed)
-            self.pidY.enable()
-            self.pidY.setSetpoint(mappedY)
-            y = self.pidYRate
+                    # 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.useYPID != False):
+                        mappedY = helpers.remap(y, -1, 1, -self.maxSpeed, self.maxSpeed)
+                        self.pidY.enable()
+                        self.pidY.setSetpoint(mappedY)
+                        y = self.pidYRate
 
+        # write manipulated values to motors
+        self.cartesian(x, y, rotation)
+
+    def cartesian(self, x, y, rotation):
         # assign speeds
         speeds = [0] * 4
         speeds[0] =  x + y + rotation # front left
@@ -123,31 +125,33 @@ def curve(self, value):
 
         return (math.sin(value) / math.sin(1));
 
-    def straight(self, duration, power):
+    def toAngle(self, angle):
         """Intended for use in auto."""
-        if hal.isSimulation() == False:
-            startTime = time.clock()
-            while (time.clock() - startTime < duration):
-                self.cartesian(0, power, 0)
-                print(time.clock())
-
-            # Stop
-            self.cartesian(0, 0, 0)
-
-    def driveToAngle(self, power, angle):
-        """Intended for use in auto."""
-        self.gyro.reset()
         self.pidAngle.setSetpoint(angle)
         self.pidAngle.enable()
 
-        while (abs(self.pidAngle.getError()) > 2):
-            print(self.pidAngle.getError())
-            self.cartesian(0, 0, -self.pidRotateRate)
-
+        if (self.pidAngle.onTarget()):
             self.pidAngle.disable()
-            self.cartesian(0, 0, 0)
-            self.gyro.reset()
-            return
+            return True
+        else:
+            self.cartesian(0, 0, self.pidRotateRate)
+            return False
 
-    def driveToPosition(self, distance):
-        print (distance)
+    def toDistance(self, distance):
+        """Intended for use in auto."""
+        if (self.toDistanceFirstCall):
+            self.encoderY.reset()
+            self.toDistanceFirstCall = False
+
+        self.pidY.setContinuous(False)
+        self.pidY.setAbsoluteTolerance(0.1)
+        self.pidY.setSetpoint(distance)
+        self.pidY.enable()
+
+        if (self.pidY.onTarget()):
+            self.pidY.disable()
+            self.toDistanceFirstCall = True
+            return True
+        else:
+            self.cartesian(0, self.pidYRate, 0)
+            return False

components/metabox.py

@@ -3,14 +3,17 @@
 """
 
 import wpilib
+import helpers
 from networktables import NetworkTables
 
 class MetaBox(object):
-    def __init__(self, encoderS, limitS, elevatorM, intakeM):
+    def __init__(self, encoderS, limitS, elevatorM, intakeM, jawsSol, pusherSol):
         self.encoder = encoderS
         self.limit = limitS
         self.elevatorM = elevatorM
         self.intakeM = intakeM
+        self.jawsSol = jawsSol
+        self.pusherSol = pusherSol
         self.elevatorTravel = 26.25
         self.floorOffset = 11.25
         self.sd = NetworkTables.getTable('SmartDashboard')
@@ -22,14 +25,67 @@ def __init__(self, encoderS, limitS, elevatorM, intakeM):
         self.sd.putNumber('elevatorI', self.pidDefault['i'])
         self.sd.putNumber('elevatorD', self.pidDefault['d'])
 
-    def run(self, heightRate, runIn, runOut):
+        # Toggles for mechanisms
+        self.runInT = helpers.timeToggle()
+        self.pushOut1T = helpers.timeToggle()
+        self.pushOut2T = helpers.timeToggle()
+        self.pushOut3T = helpers.timeToggle()
+
+    def run(self, heightRate, runIn, pushOut1, pushOut2, pushOut3):
+        '''
+        Intended to be called with a periodic loop
+        and with button toggles.
+        '''
+        # set elevator
         self.set(heightRate)
 
-        if (runIn):
+        if (runIn or self.runInT.get()):
+            # intake cube
+            self.jawsSol.set(self.jawsSol.Value.kForward)
             self.intakeM.set(1)
-        elif (runOut):
+
+            self.runInT.start(runIn, 2)
+
+        elif (pushOut1 or self.pushOut1T.get()):
+            # open jaws
+            self.jawsSol.set(self.jawsSol.Value.kForward)
+
+            self.pushOut1T.start(pushOut1, 2)
+
+        elif (pushOut2 or self.pushOut2T.get()):
+            # run wheels
+            self.running = 'pushOut2'
             self.intakeM.set(-1)
+
+            self.pushOut2T.start(pushOut2, 2)
+
+        elif (pushOut3 or self.pushOut3T.get()):
+            # use pusher
+            self.pusherSol.set(self.pusherSol.Value.kForward)
+
+            #self.pushOut3T.(pushOut3, 3)
+
+            self.pusherT = helpers.timeToggle()
+            self.wheelsT = helpers.timeToggle()
+            self.jawsT = helpers.timeToggle()
+
+            if (self.pusherT.get()):
+                # run wheels
+                self.intakeM.set(-1)
+                wheelsTimer = wpilib.Timer()
+
+                if (self.wheelsT.get()):
+                    # open jaws
+                    self.jawsSol.set(self.jawsSol.Value.kForward)
+                    jawsTimer = wpilib.timer()
+
+                    if (self.jawsT.get()):
+                        self.running = ''
+
         else:
+            # close jaws and stop intake
+            self.jawsSol.set(self.jawsSol.Value.kReverse)
+            self.pusherSol.set(self.pusherSol.Value.kReverse)
             self.intakeM.set(0)
 
     ''' Functions that want to move the elevator should call this instead of elevatorM.set() directly. '''

helpers.py

@@ -2,8 +2,29 @@
 Provides various helper functions.
 """
 
+import wpilib
 import math
 
+class timeToggle(object):
+    def __init__(self):
+        self.period = 0
+        self.timer = wpilib.Timer()
+        self.running = False
+
+    def start(self, reset, period):
+        if (self.running == False and reset):
+            self.period = period
+            self.timer.start()
+            self.running = True
+
+    def get(self):
+        if (not self.timer.hasPeriodPassed(self.period) and self.running == True): # will be true if timer still running
+            return True
+        else:
+            self.timer.stop()
+            self.running = False
+            return False
+
 def remap( x, oMin, oMax, nMin, nMax ): # thanks stackoverflow.com/a/15537393
     #range check
     if oMin == oMax:

inits.py

@@ -26,7 +26,8 @@ def __init__(self):
         self.intakeM = wpilib.Talon(Mapping.intakeM)
 
         # Soleniods
-        self.metaboxSol = wpilib.DoubleSolenoid(Mapping.metaboxSol['out'], Mapping.metaboxSol['in'])
+        self.jawsSol = wpilib.DoubleSolenoid(Mapping.jawsSol['out'], Mapping.jawsSol['in'])
+        self.pusherSol = wpilib.DoubleSolenoid(Mapping.pusherSol['out'], Mapping.pusherSol['in'])
 
         # Init sensors
         self.gyroS = wpilib.AnalogGyro(Mapping.gyroS)
@@ -37,4 +38,4 @@ def __init__(self):
         self.elevatorEncoderS.setDistancePerPulse(0.08078)
 
         self.driveYEncoderS = wpilib.Encoder(2, 3)
-        self.driveYEncoderS.setDistancePerPulse(0.1885)
+        self.driveYEncoderS.setDistancePerPulse(0.015708)

map.py

@@ -19,7 +19,9 @@ def __init__(self):
         # DIO
         self.elevatorLimitS = 0
 
-        self.metaboxSol = {'out': 0, 'in': 1}
+        # Soleniods
+        self.jawsSol = {'out': 2, 'in': 3}
+        self.pusherSol = {'out': 0, 'in': 1}
 
     """    self.frontLeftM  = 0
         self.frontRightM = 3

physics.py

@@ -0,0 +1,47 @@
+from pyfrc.physics import drivetrains
+import math
+
+
+class PhysicsEngine(object):
+    '''
+       Simulates a 4-wheel mecanum robot using Tank Drive joystick control
+    '''
+
+    def __init__(self, physics_controller):
+        '''
+            :param physics_controller: `pyfrc.physics.core.Physics` object
+                                       to communicate simulation effects to
+        '''
+
+        self.physics_controller = physics_controller
+
+        # Precompute the encoder constant
+        # -> encoder counts per revolution / wheel circumference
+        self.kEncoder = 100 / (0.5 * math.pi)
+        self.distance = 0
+
+
+    def update_sim(self, hal_data, now, tm_diff):
+        '''
+            Called when the simulation parameters for the program need to be
+            updated.
+
+            :param now: The current time as a float
+            :param tm_diff: The amount of time that has passed since the last
+                            time that this function was called
+        '''
+
+        # Simulate the drivetrain
+        # -> Remember, in the constructor we inverted the left motors, so
+        #    invert the motor values here too!
+        lr_motor = -hal_data['pwm'][6]['value']
+        rr_motor = hal_data['pwm'][5]['value']
+        lf_motor = -hal_data['pwm'][7]['value']
+        rf_motor = hal_data['pwm'][3]['value']
+
+        vx, vy, vw = drivetrains.mecanum_drivetrain(lr_motor, rr_motor, lf_motor, rf_motor)
+        self.physics_controller.vector_drive(vx, vy, vw, tm_diff)
+
+        # Update encoders
+        self.distance += vy * tm_diff
+        hal_data['encoder'][1]['count'] = int(self.distance * self.kEncoder)

robot.py

@@ -22,7 +22,7 @@ def robotInit(self):
         self.driverStation = wpilib.DriverStation.getInstance()
         self.drive = Chassis(self.C.driveTrain, self.C.gyroS, self.C.driveYEncoderS)
         self.lights = Lights()
-        self.metabox = MetaBox(self.C.elevatorEncoderS, self.C.elevatorLimitS, self.C.elevatorM, self.C.intakeM)
+        self.metabox = MetaBox(self.C.elevatorEncoderS, self.C.elevatorLimitS, self.C.elevatorM, self.C.intakeM, self.C.jawsSol, self.C.pusherSol)
         self.winch = Winch(self.C.winchM)
         self.power = Power()
 
@@ -40,13 +40,17 @@ def teleopPeriodic(self):
         """This function is called periodically during operator control."""
         '''Components'''
         # Drive
-        self.drive.cartesian(self.C.joystick.getRawAxis(0), self.C.joystick.getRawAxis(1), self.C.joystick.getRawAxis(4))
+        self.drive.run(self.C.joystick.getRawAxis(0), self.C.joystick.getRawAxis(1), self.C.joystick.getRawAxis(4))
 
         # MetaBox
-        self.metabox.run(self.C.leftJ.getY(), self.C.leftJ.getRawButton(4), self.C.leftJ.getRawButton(5))
+        self.metabox.run(self.C.leftJ.getY(),          # elevator rate of change
+                         self.C.leftJ.getRawButton(1), # run intake wheels in
+                         self.C.leftJ.getRawButton(4), # run 1st push out preset
+                         self.C.leftJ.getRawButton(3), # run 2nd push out preset
+                         self.C.leftJ.getRawButton(5)) # run 3rd push out preset
 
         # Winch
-        if (self.C.leftJ.getRawButton(3)):
+        if (self.C.leftJ.getRawButton(9)):
             self.winch.run(1)
         else:
             self.winch.run(0)
@@ -69,7 +73,22 @@ def teleopInit(self):
     def autonomousInit(self):
         """This function is run once each time the robot enters autonomous mode."""
         self.lights.run({'effect': 'flash', 'fade': True, 'speed': 400})
-        self.autonomousRoutine.run() # see autonomous.py
+        # reset gyro
+        self.C.gyroS.reset()
+
+    def autonomousPeriodic(self):
+        state = 0
+        if (state == 0):
+            if (self.drive.toDistance(6)):
+                state += 1
+        if (state == 1):
+            if (self.drive.toAngle(-45)):
+                state += 1
+        if (state == 2):
+            if (self.drive.toDistance(10)):
+                state += 1
+        print(state)
+        #self.autonomousRoutine.run() # see autonomous.py
 
 if __name__ == "__main__":
     wpilib.run(Randy)