Add basic motion profiling

requirements.txt

@@ -1,3 +1,6 @@
 pyfrc>=2018.0.0
 robotpy-ctre>=2018.0.0
 pynetworktables>=2018.0.0
+pybind11>=2.2.2
+robotpy-pathfinder>=0.2.5
+robotpy-navx>=2018.0.0

robot/autonomous/charge.py

@@ -1,16 +1,20 @@
 from magicbot.state_machine import state, timed_state, AutonomousStateMachine
-#from automations import
-#from magicbot import tunable
-from components import drive
+# from automations import
+# from magicbot import tunable
+from components import drive, trajectory_follower
 
 
 class Charge(AutonomousStateMachine):
     MODE_NAME = 'Charge'
     DEFAULT = True
 
-    drive = drive.Drive
+    drive: drive.Drive
+    follower: trajectory_follower.TrajectoryFollower
 
-    @timed_state(duration=3, first=True)
+    @state(first=True)
     def charge(self, initial_call):
-        # Move forward
-        self.drive.move(1, 0)
+        if initial_call:
+            self.follower.follow_trajectory('charge')
+
+        if not self.follower.is_following('charge'):
+            self.done()  # If using mutliple states use self.next_state(name)

robot/components/drive.py

@@ -8,7 +8,7 @@
 
 
 class Drive:
-    drivetrain = wpilib.drive.DifferentialDrive
+    drivetrain: wpilib.drive.DifferentialDrive
 
     def __init__(self):
         self.enabled = False

robot/components/trajectory_follower.py

@@ -0,0 +1,77 @@
+import pathfinder as pf
+import navx
+from typing import Tuple, List
+import wpilib
+from wpilib import drive
+
+
+class TrajectoryFollower:
+    """
+    Move along generated paths for autonomous
+    """
+    # TODO FIND THE REAL VALUES
+    WHEEL_DIAMETER = 0.5
+    KV = 1.101
+    KA = 0.164  # 0.102
+
+    drivetrain: drive.DifferentialDrive
+    navx: navx.AHRS
+    l_encoder: wpilib.Encoder
+    r_encoder: wpilib.Encoder
+    generated_trajectories: dict
+
+    def on_enable(self):
+        self._current_trajectory = None
+        self.last_difference = 0
+
+        self.left_follower = pf.followers.EncoderFollower(None)
+        self.right_follower = pf.followers.EncoderFollower(None)
+
+        self.left_follower.configurePIDVA(1.0, 0, 0.1, 1 / 10.903, 1 / 73.220)
+        self.right_follower.configurePIDVA(1.0, 0, 0.1, 1 / 10.903, 1 / 73.220)
+
+        self._cofigure_encoders()
+
+    def follow_trajectory(self, trajectory_name: str):
+        self._current_trajectory = trajectory_name
+        self.left_follower.setTrajectory(self.generated_trajectories[trajectory_name][0])
+        self.right_follower.setTrajectory(self.generated_trajectories[trajectory_name][1])
+
+        self._cofigure_encoders()
+
+    def _cofigure_encoders(self):
+        self.left_follower.configureEncoder(self.l_encoder.get(), 360, self.WHEEL_DIAMETER)
+        self.right_follower.configureEncoder(self.r_encoder.get(), 360, self.WHEEL_DIAMETER)
+
+    def is_following(self, trajectory_name):
+        return self._current_trajectory is not None and self._current_trajectory == trajectory_name
+
+    def execute(self):
+        if (self.left_follower.trajectory is None or self.right_follower.trajectory is None) or \
+           (self.left_follower.isFinished() and self.right_follower.isFinished()):
+            self._current_trajectory = None
+            return
+
+        left = self.left_follower.calculate(self.l_encoder.get())
+        right = self.right_follower.calculate(self.r_encoder.get())
+
+        gyro_heading = (
+            -self.navx.getAngle()
+        )  # Assuming the gyro is giving a value in degrees
+        desired_heading = pf.r2d(
+            self.left_follower.getHeading()
+        )  # Should also be in degrees
+
+        # This is a poor man's P controller
+        angle_difference = pf.boundHalfDegrees(desired_heading - gyro_heading)
+        turn = (1.1 * (-1.0 / 80.0) * angle_difference) + (0.05 * (angle_difference - self.last_difference))
+        # turn = 0.7 * (-1.0 / 80.0) * angle_difference
+        # turn = 0
+
+        self.last_difference = angle_difference
+
+        left += turn
+        right -= turn
+
+        # -1 is forward, so invert both values
+        self.drivetrain.tankDrive(-left, -right)

robot/physics.py

@@ -0,0 +1,60 @@
+import math
+from pyfrc.physics import motor_cfgs, tankmodel
+from pyfrc.physics.units import units
+
+
+class PhysicsEngine(object):
+    """
+    Simulates a 4-wheel 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
+
+        self.drivetrain = tankmodel.TankModel.theory(
+            motor_cfgs.MOTOR_CFG_CIM,           # motor configuration
+            80 * units.lbs,                      # robot mass
+            12.75,                              # drivetrain gear ratio
+            2,                                  # motors per side
+            17.75 * units.inch,                      # robot wheelbase
+            21.63 * units.inch,     # robot width
+            28 * units.inch,     # robot length
+            6 * units.inch                        # wheel diameter
+        )
+
+        self.kEncoder = 360 / (0.5 * math.pi)
+
+        self.l_distance = 0
+        self.r_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
+        lf_motor = hal_data['pwm'][0b00]['value']
+        rf_motor = hal_data['pwm'][0b10]['value']
+
+        # Not needed because front and rear should be in sync
+        # lr_motor = hal_data['pwm'][0b01]['value']
+        # rr_motor = hal_data['pwm'][0b11]['value']
+
+        x, y, angle = self.drivetrain.get_distance(lf_motor, rf_motor, tm_diff)
+        self.physics_controller.distance_drive(x, y, angle)
+
+        # Update encoders
+        self.l_distance += self.drivetrain.l_velocity * tm_diff
+        self.r_distance += self.drivetrain.r_velocity * tm_diff
+
+        hal_data['encoder'][0]['count'] = int(self.l_distance * self.kEncoder)
+        hal_data['encoder'][1]['count'] = int(self.r_distance * self.kEncoder)

robot/robot.py

@@ -5,15 +5,19 @@
 
 from robotpy_ext.control.button_debouncer import ButtonDebouncer
 from wpilib.buttons import JoystickButton
-from components import drive, intake
+from components import drive, intake, trajectory_follower
 import wpilib.drive
+import navx
+import math
+from trajectory_generator import load_trajectories
 
 ROT_COR = -0.145
 
 
 class Bot(magicbot.MagicRobot):
     drive: drive.Drive
     intake: intake.Intake
+    follower: trajectory_follower.TrajectoryFollower
 
     def createObjects(self):
         # Joysticks
@@ -23,13 +27,25 @@ def createObjects(self):
         #   Dig | 0/1
         #   2^1 | Left/Right
         #   2^0 | Front/Rear
-        self.lf_motor = wpilib.Victor(0b00) # =>0
-        self.lr_motor = wpilib.Victor(0b01) # =>1
-        self.rf_motor = wpilib.Victor(0b10) # =>2
-        self.rr_motor = wpilib.Victor(0b11) # =>3
+        self.lf_motor = wpilib.Victor(0b00)  # =>0
+        self.lr_motor = wpilib.Victor(0b01)  # =>1
+        self.rf_motor = wpilib.Victor(0b10)  # =>2
+        self.rr_motor = wpilib.Victor(0b11)  # =>3
 
         self.drivetrain = wpilib.drive.DifferentialDrive(wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor),
-                                                    wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor))
+                                                         wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor))
+
+        # NavX (purple board on top of the RoboRIO)
+        self.navx = navx.AHRS.create_spi()
+        self.navx.reset()
+
+        self.l_encoder = wpilib.Encoder(0, 1)
+        self.l_encoder.setDistancePerPulse((math.pi * 0.5) / 360)
+
+        self.r_encoder = wpilib.Encoder(2, 3)
+        self.r_encoder.setDistancePerPulse((math.pi * 0.5) / 360)
+
+        self.generated_trajectories = load_trajectories()
 
         self.btn_sarah = ButtonDebouncer(self.joystick, 2)
         self.sarah = False
@@ -47,16 +63,15 @@ def createObjects(self):
     def autonomous(self):
         super().autonomous()
 
-    def disabledPeriodic(self): pass
-    def disabledInit(self): pass
-    def teleopInit(self): pass
+    def teleopInit(self):
+        pass
 
     def teleopPeriodic(self):
         # Normal joysticks
-        self.drive.move(-self.joystick.getY(),self.joystick.getX())
+        self.drive.move(-self.joystick.getY(), self.joystick.getX())
 
         # Corrections for aviator joystick
-        #self.drive.move(-2*(self.joystick.getY()+.5),
+        # self.drive.move(-2*(self.joystick.getY()+.5),
         #                2*(self.joystick.getX()+.5)+ROT_COR,
         #                sarah=self.sarah)
 

robot/sim/config.json

@@ -0,0 +1,16 @@
+{
+  "pyfrc": {
+  	"robot": {
+  	  "w": 2.33,
+  	  "h": 1.80,
+      "starting_x": 1.8,
+      "starting_y": 14
+  	},
+    "pwm": {
+      "0": "lf_motor",
+      "1": "lr_motor",
+      "2": "rf_motor",
+      "3": "rr_motor"
+    }
+  }
+}

robot/trajectory_generator.py

@@ -0,0 +1,73 @@
+import os
+import pickle
+import pathfinder as pf
+import wpilib
+
+
+WHEELBASE_WIDTH = 1.479  # In feet
+TRAJECTORY_DIRECTORY = 'trajectories'
+PICKLE_FILE = os.path.join(os.path.dirname(__file__), TRAJECTORY_DIRECTORY, 'trajectories.pickle')
+
+trajectories = {
+    "diagonal_higher": [
+        pf.Waypoint(0, 0, 0),  # Waypoints are relative to first, so start at 0, 0, 0
+        pf.Waypoint(15, 8, 0)
+    ],
+    "diagonal": [
+        pf.Waypoint(0, 0, 0),  # Waypoints are relative to first, so start at 0, 0, 0
+        pf.Waypoint(15, 5, 0)
+    ],
+    "charge": [
+        pf.Waypoint(0, 0, 0),
+        pf.Waypoint(10, 0, 0)
+    ]
+}
+
+
+def load_trajectories():
+    if wpilib.RobotBase.isSimulation():
+        generated_trajectories = _generate_trajectories()
+        _write_trajectories(generated_trajectories)
+    else:
+        with open(PICKLE_FILE, 'rb') as f:
+            generated_trajectories = pickle.load(f)
+
+    return generated_trajectories
+
+
+def _write_trajectories(trajectories):
+    with open(PICKLE_FILE, 'wb') as f:
+        pickle.dump(trajectories, f)
+
+
+def _generate_trajectories():
+    generated_trajectories = {}
+
+    for trajectory_name, trajectory in trajectories.items():
+        generated_trajectory = pf.generate(
+            trajectory,
+            pf.FIT_HERMITE_CUBIC,
+            pf.SAMPLES_HIGH,
+            dt=0.02,  # 20ms
+            max_velocity=10.903,      # These are in ft/sec and
+            max_acceleration=73.220,  # set the units for distance to ft.
+            max_jerk=140
+        )[1]  # The 0th element is just info
+
+        modifier = pf.modifiers.TankModifier(generated_trajectory).modify(WHEELBASE_WIDTH)
+
+        generated_trajectories[trajectory_name] = (
+            modifier.getLeftTrajectory(),
+            modifier.getRightTrajectory()
+        )
+
+    if wpilib.RobotBase.isSimulation():
+        from pyfrc.sim import get_user_renderer
+
+        renderer = get_user_renderer()
+        if renderer:
+            renderer.draw_pathfinder_trajectory(modifier.getLeftTrajectory(), '#0000ff', offset=(-0.9, 0))
+            renderer.draw_pathfinder_trajectory(modifier.source, '#00ff00', show_dt=True)
+            renderer.draw_pathfinder_trajectory(modifier.getRightTrajectory(), '#0000ff', offset=(0.9, 0))
+
+    return generated_trajectories

test.sh

@@ -3,4 +3,4 @@ set -e
 
 python3 robot/robot.py test
 # E501 line too long
-pycodestyle . --ignore=E501
+pycodestyle robot/ --ignore=E501