From 2fc2b915beb5937f5183014e1b44c8d97d67774b Mon Sep 17 00:00:00 2001 From: Dustin Spicuzza Date: Sat, 17 Dec 2016 22:34:07 -0500 Subject: [PATCH] Add pyfrc samples --- iterative/src/robot.py | 42 +++++++++++++ iterative/tests/basic_test.py | 96 +++++++++++++++++++++++++++++ iterative/tests/pyfrc_test.py | 6 ++ physics-4wheel/src/physics.py | 41 ++++++++++++ physics-4wheel/src/robot.py | 59 ++++++++++++++++++ physics-4wheel/src/sim/config.json | 26 ++++++++ physics-4wheel/tests/pyfrc_test.py | 6 ++ physics-mecanum/src/physics.py | 44 +++++++++++++ physics-mecanum/src/robot.py | 65 +++++++++++++++++++ physics-mecanum/src/sim/config.json | 26 ++++++++ physics-mecanum/tests/pyfrc_test.py | 6 ++ physics-spi/src/physics.py | 73 ++++++++++++++++++++++ physics-spi/src/robot.py | 85 +++++++++++++++++++++++++ physics-spi/src/sim/config.json | 26 ++++++++ physics-spi/tests/pyfrc_test.py | 6 ++ physics/src/physics.py | 73 ++++++++++++++++++++++ physics/src/robot.py | 78 +++++++++++++++++++++++ physics/src/sim/config.json | 26 ++++++++ physics/tests/pyfrc_test.py | 6 ++ sample/src/robot.py | 44 +++++++++++++ sample/tests/basic_test.py | 96 +++++++++++++++++++++++++++++ sample/tests/pyfrc_test.py | 6 ++ 22 files changed, 936 insertions(+) create mode 100755 iterative/src/robot.py create mode 100644 iterative/tests/basic_test.py create mode 100644 iterative/tests/pyfrc_test.py create mode 100644 physics-4wheel/src/physics.py create mode 100755 physics-4wheel/src/robot.py create mode 100644 physics-4wheel/src/sim/config.json create mode 100644 physics-4wheel/tests/pyfrc_test.py create mode 100644 physics-mecanum/src/physics.py create mode 100755 physics-mecanum/src/robot.py create mode 100644 physics-mecanum/src/sim/config.json create mode 100644 physics-mecanum/tests/pyfrc_test.py create mode 100644 physics-spi/src/physics.py create mode 100755 physics-spi/src/robot.py create mode 100644 physics-spi/src/sim/config.json create mode 100644 physics-spi/tests/pyfrc_test.py create mode 100644 physics/src/physics.py create mode 100755 physics/src/robot.py create mode 100644 physics/src/sim/config.json create mode 100644 physics/tests/pyfrc_test.py create mode 100755 sample/src/robot.py create mode 100644 sample/tests/basic_test.py create mode 100644 sample/tests/pyfrc_test.py diff --git a/iterative/src/robot.py b/iterative/src/robot.py new file mode 100755 index 0000000..6b72286 --- /dev/null +++ b/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) + diff --git a/iterative/tests/basic_test.py b/iterative/tests/basic_test.py new file mode 100644 index 0000000..94a635d --- /dev/null +++ b/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) + diff --git a/iterative/tests/pyfrc_test.py b/iterative/tests/pyfrc_test.py new file mode 100644 index 0000000..90ae673 --- /dev/null +++ b/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 * diff --git a/physics-4wheel/src/physics.py b/physics-4wheel/src/physics.py new file mode 100644 index 0000000..6dd21da --- /dev/null +++ b/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) diff --git a/physics-4wheel/src/robot.py b/physics-4wheel/src/robot.py new file mode 100755 index 0000000..ce8a8d3 --- /dev/null +++ b/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) + diff --git a/physics-4wheel/src/sim/config.json b/physics-4wheel/src/sim/config.json new file mode 100644 index 0000000..a7e16ed --- /dev/null +++ b/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] ] } + ] + } + } +} \ No newline at end of file diff --git a/physics-4wheel/tests/pyfrc_test.py b/physics-4wheel/tests/pyfrc_test.py new file mode 100644 index 0000000..90ae673 --- /dev/null +++ b/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 * diff --git a/physics-mecanum/src/physics.py b/physics-mecanum/src/physics.py new file mode 100644 index 0000000..f9296bf --- /dev/null +++ b/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) + diff --git a/physics-mecanum/src/robot.py b/physics-mecanum/src/robot.py new file mode 100755 index 0000000..f40cdd0 --- /dev/null +++ b/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) + diff --git a/physics-mecanum/src/sim/config.json b/physics-mecanum/src/sim/config.json new file mode 100644 index 0000000..a7e16ed --- /dev/null +++ b/physics-mecanum/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] ] } + ] + } + } +} \ No newline at end of file diff --git a/physics-mecanum/tests/pyfrc_test.py b/physics-mecanum/tests/pyfrc_test.py new file mode 100644 index 0000000..90ae673 --- /dev/null +++ b/physics-mecanum/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 * diff --git a/physics-spi/src/physics.py b/physics-spi/src/physics.py new file mode 100644 index 0000000..dcd9174 --- /dev/null +++ b/physics-spi/src/physics.py @@ -0,0 +1,73 @@ +# +# See the documentation for more details on how this works +# +# The idea here is you provide a simulation object that overrides specific +# pieces of WPILib, and modifies motors/sensors accordingly depending on the +# state of the simulation. An example of this would be measuring a motor +# moving for a set period of time, and then changing a limit switch to turn +# on after that period of time. This can help you do more complex simulations +# of your robot code without too much extra effort. +# +# NOTE: THIS API IS ALPHA AND WILL MOST LIKELY CHANGE! +# ... if you have better ideas on how to implement, submit a patch! +# + +from pyfrc.physics import drivetrains + + +class PhysicsEngine(object): + ''' + Simulates a motor moving something that strikes two limit switches, + one on each end of the track. Obviously, this is not particularly + realistic, but it's good enough to illustrate the point + ''' + + def __init__(self, physics_controller): + ''' + :param physics_controller: `pyfrc.physics.core.PhysicsInterface` object + to communicate simulation effects to + ''' + + self.physics_controller = physics_controller + self.position = 0 + + self.physics_controller.add_device_gyro_channel('adxrs450_spi_0_angle') + + 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 + l_motor = hal_data['pwm'][1]['value'] + r_motor = hal_data['pwm'][2]['value'] + + speed, rotation = drivetrains.two_motor_drivetrain(l_motor, r_motor) + self.physics_controller.drive(speed, rotation, tm_diff) + + + # update position (use tm_diff so the rate is constant) + self.position += hal_data['pwm'][4]['value'] * tm_diff * 3 + + # update limit switches based on position + if self.position <= 0: + switch1 = True + switch2 = False + + elif self.position > 10: + switch1 = False + switch2 = True + + else: + switch1 = False + switch2 = False + + # set values here + hal_data['dio'][1]['value'] = switch1 + hal_data['dio'][2]['value'] = switch2 + hal_data['analog_in'][2]['voltage'] = self.position diff --git a/physics-spi/src/robot.py b/physics-spi/src/robot.py new file mode 100755 index 0000000..e647659 --- /dev/null +++ b/physics-spi/src/robot.py @@ -0,0 +1,85 @@ +#!/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.l_motor = wpilib.Jaguar(1) + self.r_motor = wpilib.Jaguar(2) + + # Position gets automatically updated as robot moves + self.gyro = wpilib.ADXRS450_Gyro() + + self.robot_drive = wpilib.RobotDrive(self.l_motor, self.r_motor) + + self.motor = wpilib.Jaguar(4) + + self.limit1 = wpilib.DigitalInput(1) + self.limit2 = wpilib.DigitalInput(2) + + self.position = wpilib.AnalogInput(2) + + 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''' + + timer = wpilib.Timer() + timer.start() + + while self.isOperatorControl() and self.isEnabled(): + + if timer.hasPeriodPassed(1.0): + print("Gyro:", self.gyro.getAngle()) + + # This is where the data ends up.. + #from hal_impl.data import hal_data + #print(hal_data['robot']) + + self.robot_drive.arcadeDrive(self.lstick) + + # Move a motor with a Joystick + y = self.rstick.getY() + + # stop movement backwards when 1 is on + if self.limit1.get(): + y = max(0, y) + + # stop movement forwards when 2 is on + if self.limit2.get(): + y = min(0, y) + + self.motor.set(y) + + wpilib.Timer.delay(0.04) + +if __name__ == '__main__': + + wpilib.run(MyRobot, + physics_enabled=True) + diff --git a/physics-spi/src/sim/config.json b/physics-spi/src/sim/config.json new file mode 100644 index 0000000..a7e16ed --- /dev/null +++ b/physics-spi/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] ] } + ] + } + } +} \ No newline at end of file diff --git a/physics-spi/tests/pyfrc_test.py b/physics-spi/tests/pyfrc_test.py new file mode 100644 index 0000000..90ae673 --- /dev/null +++ b/physics-spi/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 * diff --git a/physics/src/physics.py b/physics/src/physics.py new file mode 100644 index 0000000..778ec80 --- /dev/null +++ b/physics/src/physics.py @@ -0,0 +1,73 @@ +# +# See the documentation for more details on how this works +# +# The idea here is you provide a simulation object that overrides specific +# pieces of WPILib, and modifies motors/sensors accordingly depending on the +# state of the simulation. An example of this would be measuring a motor +# moving for a set period of time, and then changing a limit switch to turn +# on after that period of time. This can help you do more complex simulations +# of your robot code without too much extra effort. +# +# NOTE: THIS API IS ALPHA AND WILL MOST LIKELY CHANGE! +# ... if you have better ideas on how to implement, submit a patch! +# + +from pyfrc.physics import drivetrains + + +class PhysicsEngine(object): + ''' + Simulates a motor moving something that strikes two limit switches, + one on each end of the track. Obviously, this is not particularly + realistic, but it's good enough to illustrate the point + ''' + + def __init__(self, physics_controller): + ''' + :param physics_controller: `pyfrc.physics.core.PhysicsInterface` object + to communicate simulation effects to + ''' + + self.physics_controller = physics_controller + self.position = 0 + + 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 + l_motor = hal_data['pwm'][1]['value'] + r_motor = hal_data['pwm'][2]['value'] + + speed, rotation = drivetrains.two_motor_drivetrain(l_motor, r_motor) + self.physics_controller.drive(speed, rotation, tm_diff) + + + # update position (use tm_diff so the rate is constant) + self.position += hal_data['pwm'][4]['value'] * tm_diff * 3 + + # update limit switches based on position + if self.position <= 0: + switch1 = True + switch2 = False + + elif self.position > 10: + switch1 = False + switch2 = True + + else: + switch1 = False + switch2 = False + + # set values here + hal_data['dio'][1]['value'] = switch1 + hal_data['dio'][2]['value'] = switch2 + hal_data['analog_in'][2]['voltage'] = self.position diff --git a/physics/src/robot.py b/physics/src/robot.py new file mode 100755 index 0000000..f52264c --- /dev/null +++ b/physics/src/robot.py @@ -0,0 +1,78 @@ +#!/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.l_motor = wpilib.Jaguar(1) + self.r_motor = wpilib.Jaguar(2) + + # Position gets automatically updated as robot moves + self.gyro = wpilib.AnalogGyro(1) + + self.robot_drive = wpilib.RobotDrive(self.l_motor, self.r_motor) + + self.motor = wpilib.Jaguar(4) + + self.limit1 = wpilib.DigitalInput(1) + self.limit2 = wpilib.DigitalInput(2) + + self.position = wpilib.AnalogInput(2) + + 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''' + + timer = wpilib.Timer() + timer.start() + + while self.isOperatorControl() and self.isEnabled(): + + self.robot_drive.arcadeDrive(self.lstick) + + # Move a motor with a Joystick + y = self.rstick.getY() + + # stop movement backwards when 1 is on + if self.limit1.get(): + y = max(0, y) + + # stop movement forwards when 2 is on + if self.limit2.get(): + y = min(0, y) + + self.motor.set(y) + + wpilib.Timer.delay(0.04) + +if __name__ == '__main__': + + wpilib.run(MyRobot, + physics_enabled=True) + diff --git a/physics/src/sim/config.json b/physics/src/sim/config.json new file mode 100644 index 0000000..a7e16ed --- /dev/null +++ b/physics/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] ] } + ] + } + } +} \ No newline at end of file diff --git a/physics/tests/pyfrc_test.py b/physics/tests/pyfrc_test.py new file mode 100644 index 0000000..90ae673 --- /dev/null +++ b/physics/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 * diff --git a/sample/src/robot.py b/sample/src/robot.py new file mode 100755 index 0000000..4732b45 --- /dev/null +++ b/sample/src/robot.py @@ -0,0 +1,44 @@ +#!/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(1) + self.motor = wpilib.Jaguar(8) + + 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''' + + while self.isAutonomous() and self.isEnabled(): + wpilib.Timer.delay(0.01) + + def operatorControl(self): + '''Called when operation control mode is enabled''' + + timer = wpilib.Timer() + timer.start() + + while self.isOperatorControl() and self.isEnabled(): + + # Move a motor with a Joystick + self.motor.set(self.lstick.getY()) + + if timer.hasPeriodPassed(1.0): + print("Analog 8: %s" % self.ds.getBatteryVoltage()) + + wpilib.Timer.delay(0.02) + +if __name__ == '__main__': + wpilib.run(MyRobot) + diff --git a/sample/tests/basic_test.py b/sample/tests/basic_test.py new file mode 100644 index 0000000..94a635d --- /dev/null +++ b/sample/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) + diff --git a/sample/tests/pyfrc_test.py b/sample/tests/pyfrc_test.py new file mode 100644 index 0000000..90ae673 --- /dev/null +++ b/sample/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 *