Add pyfrc samples

iterative/src/robot.py

@@ -0,0 +1,42 @@
+#!/usr/bin/env python3
+
+import wpilib
+
+class MyRobot(wpilib.IterativeRobot):
+    '''Main robot class'''
+
+    def robotInit(self):
+        '''Robot-wide initialization code should go here'''
+
+        self.lstick = wpilib.Joystick(1)
+        self.motor = wpilib.Jaguar(3)
+
+    def autonomousInit(self):
+        '''Called only at the beginning of autonomous mode'''
+        pass
+
+    def autonomousPeriodic(self):
+        '''Called every 20ms in autonomous mode'''
+        pass
+
+    def disabledInit(self):
+        '''Called only at the beginning of disabled mode'''
+        pass
+
+    def disabledPeriodic(self):
+        '''Called every 20ms in disabled mode'''
+        pass
+
+    def teleopInit(self):
+        '''Called only at the beginning of teleoperated mode'''
+        pass
+
+    def teleopPeriodic(self):
+        '''Called every 20ms in teleoperated mode'''
+
+        # Move a motor with a Joystick
+        self.motor.set(self.lstick.getY())
+
+if __name__ == '__main__':
+    wpilib.run(MyRobot)
+

iterative/tests/basic_test.py

@@ -0,0 +1,96 @@
+#
+# This is a py.test based testing system. Anything you can do with
+# py.test, you can do with this too. There are a few magic parameters
+# provided as fixtures that will allow your tests to access the robot
+# code and stuff
+#
+#    control - the wpilib.internal module
+#    fake_time - the FAKETIME object that controls time
+#    robot - This is whatever is returned from the run function in robot.py
+#    robot_file - The filename of robot.py
+#    robot_path - The directory name that robot.py is located in
+#    wpilib - This is the wpilib module
+#
+
+
+#
+# Each of these functions is an individual test run by py.test. Global
+# wpilib state is cleaned up between each test run, and a new robot
+# object is created each time
+#
+
+def test_autonomous(control, fake_time, robot):
+
+    # run autonomous mode for 10 seconds
+    control.set_autonomous(enabled=True)
+    control.run_test(lambda tm: tm < 15)
+
+    # make sure autonomous mode ran for 15 seconds
+    assert int(fake_time.get()) == 15
+
+
+def test_disabled(control, fake_time, robot):
+
+    # run disabled mode for 5 seconds
+    control.set_autonomous(enabled=False)
+    control.run_test(lambda tm: tm < 5)
+
+    # make sure disabled mode ran for 5 seconds
+    assert int(fake_time.get()) == 5
+
+
+def test_operator_control(control, robot, hal_data):
+
+    class TestController:
+        '''This object is only used for this test'''
+
+        step_count = 0
+
+        # Use two values because we're never quite sure when the value will
+        # be changed by the robot... there's probably a better way to do this
+        expected_value = 0
+
+        def on_step(self, tm):
+            '''
+                Continue operator control for 1000 simulation steps. Each step
+                represents roughly 20ms of fake time.
+                
+                The idea is to change the joystick/other inputs, and see if the 
+                robot motors/etc respond the way that we expect. 
+                
+                Keep in mind that when you set a value, the robot code does not
+                see the value until after this function returns AND the driver
+                station delivers a new packet. Therefore when you use assert to
+                check a motor value, you have to check to see that it matches
+                the previous value that you set on the inputs, not the current
+                value.
+                
+                :param tm: The current robot time in seconds
+            '''
+            self.step_count += 1
+
+            pwm_val = hal_data['pwm'][8]['value']
+            if pwm_val is not None:
+
+                # motor value is equal to the previous value of the stick
+                # -> Note that the PWM value isn't exact, because it was converted to
+                #    a raw PWM value and then back to -1 to 1
+                assert abs(pwm_val - self.expected_value) < 0.1
+
+                # We do this so that we only check the value when it changes
+                hal_data['pwm'][8]['value'] = None
+
+                # set the stick value based on time
+
+                self.expected_value = (tm % 2.0) - 1.0
+                print("Set value", self.expected_value)
+                hal_data['joysticks'][1]['axes'][1] = self.expected_value
+
+            return not self.step_count == 1000
+
+    # Initialize
+    hal_data['pwm'][8]['value'] = None
+
+    control.set_operator_control(enabled=True)
+    control.run_test(TestController)
+

iterative/tests/pyfrc_test.py

@@ -0,0 +1,6 @@
+'''
+    This test module imports tests that come with pyfrc, and can be used
+    to test basic functionality of just about any robot.
+'''
+
+from pyfrc.tests import *

physics-4wheel/src/physics.py

@@ -0,0 +1,41 @@
+#
+# See the notes for the other physics sample
+#
+
+
+from pyfrc.physics import drivetrains
+
+
+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.physics_controller.add_analog_gyro_channel(1)
+
+    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
+        lr_motor = hal_data['pwm'][1]['value']
+        rr_motor = hal_data['pwm'][2]['value']
+        lf_motor = hal_data['pwm'][3]['value']
+        rf_motor = hal_data['pwm'][4]['value']
+
+        speed, rotation = drivetrains.four_motor_drivetrain(lr_motor, rr_motor, lf_motor, rf_motor)
+        self.physics_controller.drive(speed, rotation, tm_diff)

physics-4wheel/src/robot.py

@@ -0,0 +1,59 @@
+#!/usr/bin/env python3
+
+import wpilib
+
+
+class MyRobot(wpilib.SampleRobot):
+    '''Main robot class'''
+
+    def robotInit(self):
+        '''Robot-wide initialization code should go here'''
+
+        self.lstick = wpilib.Joystick(0)
+        self.rstick = wpilib.Joystick(1)
+
+        self.lr_motor = wpilib.Jaguar(1)
+        self.rr_motor = wpilib.Jaguar(2)
+        self.lf_motor = wpilib.Jaguar(3)
+        self.rf_motor = wpilib.Jaguar(4)
+
+        self.robot_drive = wpilib.RobotDrive(self.lf_motor, self.lr_motor,
+                                             self.rf_motor, self.rr_motor)
+
+        # Position gets automatically updated as robot moves
+        self.gyro = wpilib.AnalogGyro(1)
+
+    def disabled(self):
+        '''Called when the robot is disabled'''
+        while self.isDisabled():
+            wpilib.Timer.delay(0.01)
+
+    def autonomous(self):
+        '''Called when autonomous mode is enabled'''
+
+        timer = wpilib.Timer()
+        timer.start()
+
+        while self.isAutonomous() and self.isEnabled():
+
+            if timer.get() < 2.0:
+                self.robot_drive.arcadeDrive(-1.0, -.3)
+            else:
+                self.robot_drive.arcadeDrive(0, 0)
+
+            wpilib.Timer.delay(0.01)
+
+    def operatorControl(self):
+        '''Called when operation control mode is enabled'''
+
+        while self.isOperatorControl() and self.isEnabled():
+
+            self.robot_drive.tankDrive(self.lstick, self.rstick)
+
+            wpilib.Timer.delay(0.04)
+
+
+if __name__ == '__main__':
+    wpilib.run(MyRobot,
+               physics_enabled=True)
+

physics-4wheel/src/sim/config.json

@@ -0,0 +1,26 @@
+{
+  "pyfrc": {
+
+  	"robot": {
+  	  "w": 2,
+  	  "h": 3,
+      "starting_x": 2, 
+      "starting_y": 20,
+      "starting_angle": 0
+  	},
+
+  	"field": {
+      "w": 25,
+      "h": 27,
+      "px_per_ft": 10,
+
+      "objects": [
+      	{ "color": "grey",
+      	  "points": [ [7.25,11.21], [10.333,11.21], [10.333,27], [7.25,27] ] },
+
+      	{ "color": "grey",
+      	  "points": [ [16.83, 0], [19.90, 0], [19.90, 15.75], [16.83, 15.75] ] }
+      ]
+    }
+  }
+}

physics-4wheel/tests/pyfrc_test.py

@@ -0,0 +1,6 @@
+'''
+    This test module imports tests that come with pyfrc, and can be used
+    to test basic functionality of just about any robot.
+'''
+
+from pyfrc.tests import *

physics-mecanum/src/physics.py

@@ -0,0 +1,44 @@
+#
+# See the notes for the other physics sample
+#
+
+
+from pyfrc.physics import drivetrains
+
+
+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
+        self.physics_controller.add_analog_gyro_channel(1)
+
+
+    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'][1]['value']
+        rr_motor = hal_data['pwm'][2]['value']
+        lf_motor = -hal_data['pwm'][3]['value']
+        rf_motor = hal_data['pwm'][4]['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)
+

physics-mecanum/src/robot.py

@@ -0,0 +1,65 @@
+#!/usr/bin/env python3
+
+import wpilib
+
+class MyRobot(wpilib.SampleRobot):
+    '''Main robot class'''
+
+    def robotInit(self):
+        '''Robot-wide initialization code should go here'''
+
+        self.lstick = wpilib.Joystick(0)
+        self.rstick = wpilib.Joystick(1)
+
+        self.lr_motor = wpilib.Jaguar(1)
+        self.rr_motor = wpilib.Jaguar(2)
+        self.lf_motor = wpilib.Jaguar(3)
+        self.rf_motor = wpilib.Jaguar(4)
+
+        self.robot_drive = wpilib.RobotDrive(self.lf_motor, self.lr_motor,
+                                             self.rf_motor, self.rr_motor)
+
+        # The output function of a mecanum drive robot is always
+        # +1 for all output wheels. However, traditionally wired
+        # robots will be -1 on the left, 1 on the right.
+        self.robot_drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kFrontLeft, True)
+        self.robot_drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kRearLeft, True)
+
+        # Position gets automatically updated as robot moves
+        self.gyro = wpilib.AnalogGyro(1)
+
+    def disabled(self):
+        '''Called when the robot is disabled'''
+        while self.isDisabled():
+            wpilib.Timer.delay(0.01)
+
+    def autonomous(self):
+        '''Called when autonomous mode is enabled'''
+
+        timer = wpilib.Timer()
+        timer.start()
+
+        while self.isAutonomous() and self.isEnabled():
+
+            if timer.get() < 2.0:
+                self.robot_drive.mecanumDrive_Cartesian(0, -1, 1, 0)
+            else:
+                self.robot_drive.mecanumDrive_Cartesian(0, 0, 0, 0)
+
+            wpilib.Timer.delay(0.01)
+
+    def operatorControl(self):
+        '''Called when operation control mode is enabled'''
+
+        while self.isOperatorControl() and self.isEnabled():
+
+            self.robot_drive.mecanumDrive_Cartesian(self.lstick.getX(), self.lstick.getY(), self.rstick.getX(), 0)
+
+            wpilib.Timer.delay(0.04)
+
+
+if __name__ == '__main__':
+
+    wpilib.run(MyRobot,
+               physics_enabled=True)
+