Update auto

autonomous.py

@@ -4,14 +4,52 @@
 import time
 import wpilib
 import hal
+from paths import Paths
+import pathfinder as pf
 
 class Autonomous(object):
-    def __init__(self, drive, driverStation):
+    def __init__(self, drive, encoder, gyro, driverStation):
         self.drive = drive
+        self.encoder = encoder
+        self.gyro = gyro
         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
+
+        # 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):
+        print(self.encoder.get())
+        velocity = self.follower.calculate(-self.encoder.get())
+
+        gyro_heading = -self.gyro.getAngle()
+        desired_heading = pf.r2d(self.follower.getHeading())
+
+        angleDifference = pf.boundHalfDegrees(desired_heading - gyro_heading)
+        turn = 0.8 * (-1.0/80.0) * angleDifference
+
+        print(turn)
+
+        self.drive.cartesian(0, -velocity, turn)
         """
         targetScale = True
         targetSwitch = True
@@ -20,9 +58,17 @@ def run(self):
         scale = 'Left'
 
         # Example:
-        @state(first=True)
-        def driveToAngle(self):
-            self.drive.driveToAngle(60)
+                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)
 
         if (self.driveToAngle(60) == True)
 

paths.py

@@ -0,0 +1,54 @@
+import os.path
+import pickle
+import pathfinder as pf
+import wpilib
+import math
+# because of a quirk in pyfrc, this must be in a subdirectory
+# or the file won't get copied over to the robot
+pickle_file = os.path.join(os.path.dirname(__file__), 'trajectory.pickle')
+
+class Paths(object):
+    def __init__(self):
+        if wpilib.RobotBase.isSimulation():
+            # Generate trajectories during testing
+            paths = {
+                'forward': [
+                    pf.Waypoint(-11, 0, 0),
+                    pf.Waypoint(0, 0, 0)
+                ],
+
+                'left': [
+                    pf.Waypoint(0, 0, 0),
+                    pf.Waypoint(3, -10, 0),
+                    pf.Waypoint(18, -5, math.radians(-90)),
+                    #pf.Waypoint(18, 0, 0)
+                    #pf.Waypoint(14, 14, 0)
+                    #pf.Waypoint(10, 15, 0),
+                    #pf.Waypoint(14, 14, math.radians(-90))
+                    #pf.Waypoint(-8, -5, 0),
+                    #pf.Waypoint(-4, -1, math.radians(-45.0)),   # Waypoint @ x=-4, y=-1, exit angle=-45 degrees
+                    #pf.Waypoint(-2, -2, 0),                     # Waypoint @ x=-2, y=-2, exit angle=0 radians
+                ]
+            }
+
+            trajectories = {}
+
+            for path in paths:
+                info, trajectory = pf.generate(paths[path],
+                                               pf.FIT_HERMITE_CUBIC,
+                                               pf.SAMPLES_HIGH,
+                                               dt = 0.05,
+                                               max_velocity = 1.7,
+                                               max_acceleration = 2,
+                                               max_jerk = 100
+                                               )
+                trajectories[path] = trajectory
+
+                self.trajectories = trajectories
+
+            # and then write it out
+            with open(pickle_file, 'wb') as fp:
+                pickle.dump(self.trajectories, fp)
+        else:
+            with open(pickle_file, 'rb') as fp:
+                self.trajectories = pickle.load(fp)

robot.py

@@ -26,8 +26,6 @@ def robotInit(self):
         self.winch = Winch(self.C.winchM)
         self.power = Power()
 
-        self.autonomousRoutine = Autonomous(self.drive, self.driverStation)
-
         # Joysticks
         self.C.joystick = wpilib.XboxController(0)
         self.C.leftJ = wpilib.Joystick(1)
@@ -76,19 +74,11 @@ def autonomousInit(self):
         # reset gyro
         self.C.gyroS.reset()
 
+        # Init autonomous
+        self.autonomousRoutine = Autonomous(self.drive, self.C.driveYEncoderS, self.C.gyroS, self.driverStation)
+
     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
+        self.autonomousRoutine.run() # see autonomous.py
 
 if __name__ == "__main__":
     wpilib.run(Randy)

sim/config.json

@@ -2,10 +2,10 @@
   "pyfrc": {
 
   	"robot": {
-  	  "w": 3,
-  	  "h": 2,
-      "starting_x": 1.8,
-      "starting_y": 14
+  	  "w": 2.67,
+  	  "h": 2.33,
+      "starting_x": 1.335,
+      "starting_y": 18.5
   	}
   }
 }