Enable jaws in auto

autonomous.py

@@ -1,63 +1,51 @@
 '''
 Runs the auto routine. Called once.
 '''
-import time
 import wpilib
-import hal
-from paths import Paths
-import pathfinder as pf
 
 class Autonomous(object):
-    def __init__(self, drive, encoder, gyro, driverStation):
+    def __init__(self, drive, encoder, gyro, metabox, driverStation):
         self.drive = drive
         self.encoder = encoder
         self.gyro = gyro
+        self.metabox = metabox
         self.driverStation = driverStation
-        self.timer = wpilib.Timer()
-        self.paths = Paths()
-
-        mode = 'left'
-
-        modifier = pf.modifiers.TankModifier(self.paths.trajectories[mode]).modify(2.667)
-
-        trajectory = modifier.getLeftTrajectory()
-
-        follower = pf.followers.EncoderFollower(trajectory)
-        follower.configureEncoder(self.encoder.get(), 100, 0.5)
-        follower.configurePIDVA(1.0, 0.0, 0.0, 1 / 5, 0)
-
-        self.follower = follower
 
         self.state = 0
 
-        # This code renders the followed path on the field in simulation (requires pyfrc 2018.2.0+)
-        if wpilib.RobotBase.isSimulation():
-            from pyfrc.sim import get_user_renderer
-            renderer = get_user_renderer()
-            if renderer:
-                renderer.draw_pathfinder_trajectory(trajectory, color='#0000ff', offset=(-1,0))
-                renderer.draw_pathfinder_trajectory(modifier.source, color='#00ff00', show_dt=1.0, dt_offset=0.0)
-                renderer.draw_pathfinder_trajectory(trajectory, color='#0000ff', offset=(1,0))
-
     def run(self):
         '''
-        velocity = self.follower.calculate(self.encoder.get())
+        if (self.state == 0):
+            if (self.drive.toDistance(4)):
+                self.state += 1
+        if (self.state == 1):
+            if (self.drive.toAngle(90)):
+                self.state += 1
 
-        gyro_heading = -self.gyro.getAngle()
-        desired_heading = pf.r2d(self.follower.getHeading())
+        if (self.state == 2):
+            if (self.drive.toDistance(4)):
+                self.state += 1
+        if (self.state == 3):
+            if (self.drive.toAngle(180)):
+                self.state += 1
 
-        angleDifference = pf.boundHalfDegrees(desired_heading - gyro_heading)
-        turn = 0.9 * (-1.0/80.0) * angleDifference
+        if (self.state == 4):
+            if (self.drive.toDistance(4)):
+                self.state += 1
+        if (self.state == 5):
+            if (self.drive.toAngle(270)):
+                self.state += 1
 
-        self.drive.cartesian(0, velocity, turn)
+        if (self.state == 6):
+            if (self.drive.toDistance(4)):
+                self.state += 1
+        if (self.state == 7):
+            if (self.drive.toAngle(360)):
+                self.state += 1
         '''
-
         if (self.state == 0):
-            if (self.drive.toDistance(6)):
+            if (self.metabox.runOutAuto(2)):
                 self.state += 1
         if (self.state == 1):
-            if (self.drive.toAngle(-45)):
-                self.state += 1
-        if (self.state == 2):
-            if (self.drive.toDistance(10)):
+            if (self.drive.toDistance(4)):
                 self.state += 1

components/chassis.py

@@ -22,21 +22,20 @@ def __init__(self, drive, gyro, encoderY):
         # PID loop for angle
         self.useAnglePID = False
 
-        self.pidAngleDefault = {'p': 0.03, 'i': 0, 'd': 0.1}
+        self.pidAngleDefault = {'p': 0.01, 'i': 0, 'd': 0.004}
         self.sd.putNumber('pidAngleP', self.pidAngleDefault['p'])
         self.sd.putNumber('pidAngleI', self.pidAngleDefault['i'])
         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.setOutputRange(-1.0, 1.0)
         self.pidAngle.setAbsoluteTolerance(2)
         self.pidRotateRate = 0
         self.wasRotating = False
 
         # PID loop for Cartesian Y direction
         self.useYPID = False
 
-        self.pidYDefault = {'p': 0.08, 'i': 0, 'd': 0}
+        self.pidYDefault = {'p': 0.15, 'i': 0, 'd': 0.05}
         self.sd.putNumber('pidYP', self.pidYDefault['p'])
         self.sd.putNumber('pidYI', self.pidYDefault['i'])
         self.sd.putNumber('pidYD', self.pidYDefault['d'])
@@ -45,6 +44,7 @@ def __init__(self, drive, gyro, encoderY):
         self.pidYRate = 0
 
         self.toDistanceFirstCall = True
+        self.toAngleFirstCall = True
 
     def run(self, x, y, rotation):
         '''Intended for use in telelop. Use .cartesian for auto.'''
@@ -110,9 +110,9 @@ def updateAnglePID(self, value):
         self.pidRotateRate = value
 
     def updateYPID(self, value):
-        self.pidY.setP(self.sd.getNumber('pidP', self.pidYDefault['p']))
-        self.pidY.setI(self.sd.getNumber('pidI', self.pidYDefault['i']))
-        self.pidY.setD(self.sd.getNumber('pidD', self.pidYDefault['d']))
+        self.pidY.setP(self.sd.getNumber('pidYP', self.pidYDefault['p']))
+        self.pidY.setI(self.sd.getNumber('pidYI', self.pidYDefault['i']))
+        self.pidY.setD(self.sd.getNumber('pidYD', self.pidYDefault['d']))
 
         self.pidYRate = value
 
@@ -125,16 +125,23 @@ def curve(self, value):
 
         return (math.sin(value) / math.sin(1));
 
-    def toAngle(self, angle):
+    def toAngle(self, angle, reset=False):
         """Intended for use in auto."""
+        if (self.toAngleFirstCall and reset == True):
+            self.gyro.reset()
+            self.toAngleFirstCall = False
+
         self.pidAngle.setSetpoint(angle)
         self.pidAngle.enable()
 
-        if (self.pidAngle.onTarget()):
+        print(self.pidAngle.getError())
+
+        if (self.pidAngle.getError() < 2):
             self.pidAngle.disable()
+            self.toAngleFirstCall = True
             return True
         else:
-            self.cartesian(0, 0, self.pidRotateRate)
+            self.cartesian(0, 0, -self.pidRotateRate)
             return False
 
     def toDistance(self, distance):
@@ -146,7 +153,7 @@ def toDistance(self, distance):
         self.pidY.setContinuous(False)
         self.pidY.setSetpoint(distance)
         self.pidY.enable()
-        
+
         print(self.pidY.getError())
 
         if (self.pidY.getError() < 0.2):

components/metabox.py

@@ -29,12 +29,15 @@ def __init__(self, elevatorEncoder, jawsEncoder, elevatorLimitS, jawsLimitS, jaw
         self.sd.putNumber('elevatorI', self.pidDefault['i'])
         self.sd.putNumber('elevatorD', self.pidDefault['d'])
 
-    def run(self, heightRate, runIn, open, runOut, bottom, angle):
+        self.timer = wpilib.Timer()
+        self.autoActionStarted = False
+
+    def run(self, heightRate, runIn, open, runOut, bottom, angle, timed=False):
         '''
         Intended to be called with a periodic loop
         and with button toggles.
         '''
-        
+
         if (runIn and self.jawsLimitS.get() == False):
             self.intakeM.set(1)
         elif (runOut):
@@ -54,6 +57,20 @@ def run(self, heightRate, runIn, open, runOut, bottom, angle):
 
         self.jawsM.set(helpers.deadband(angle, 0.1))
 
+    def runOutAuto(self, time):
+        if (self.autoActionStarted == False):
+            self.timer.start()
+            self.autoActionStarted = True
+
+        if (self.timer.hasPeriodPassed(time)):
+            self.autoActionStarted = False
+            self.intakeM.set(0)
+            return True
+        else:
+            self.intakeM.set(-1)
+            return False
+
+
     ''' Functions that want to move the elevator should call this instead of elevatorM.set() directly. '''
     def setElevator(self, value):
         if ((self.elevatorLimit.get() == False or (self.elevatorLimit.get() == True and value < 0)) and self.getEncoder() > 0):

physics.py

@@ -34,10 +34,10 @@ def update_sim(self, hal_data, now, tm_diff):
         # 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']
+        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)

robot.py

@@ -74,7 +74,6 @@ def teleopInit(self):
         """This function is run once each time the robot enters teleop mode."""
         # reset gyro
         self.C.gyroS.reset()
-
         # reset encoder
         self.C.driveYEncoderS.reset()
 
@@ -83,15 +82,20 @@ def autonomousInit(self):
         self.lights.run({'effect': 'flash', 'fade': True, 'speed': 400})
         # reset gyro
         self.C.gyroS.reset()
-
         # reset encoder
         self.C.driveYEncoderS.reset()
 
         # Init autonomous
-        self.autonomousRoutine = Autonomous(self.drive, self.C.driveYEncoderS, self.C.gyroS, self.driverStation)
+        self.autonomousRoutine = Autonomous(self.drive, self.C.driveYEncoderS, self.C.gyroS, self.metabox, self.driverStation)
 
     def autonomousPeriodic(self):
         self.autonomousRoutine.run() # see autonomous.py
 
+    def test(self):
+        # reset gyro
+        self.C.gyroS.reset()
+        # reset encoder
+        self.C.driveYEncoderS.reset()
+
 if __name__ == "__main__":
     wpilib.run(Randy)