Skip to content
This repository has been archived by the owner on Sep 26, 2023. It is now read-only.

Commit

Permalink
Fix styling issues in new files
Browse files Browse the repository at this point in the history
  • Loading branch information
AndrewLester committed Jan 6, 2019
1 parent bfd3fdc commit b1bef71
Show file tree
Hide file tree
Showing 7 changed files with 58 additions and 68 deletions.
2 changes: 1 addition & 1 deletion robot/autonomous/charge.py
Original file line number Diff line number Diff line change
Expand Up @@ -17,4 +17,4 @@ def charge(self, 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)
self.done() # If using mutliple states use self.next_state(name)
22 changes: 11 additions & 11 deletions robot/components/trajectory_follower.py
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ class TrajectoryFollower:
# TODO FIND THE REAL VALUES
WHEEL_DIAMETER = 0.5
KV = 1.101
KA = 0.225 # 0.102
KA = 0.225 # 0.102

drivetrain: drive.DifferentialDrive
navx: navx.AHRS
Expand All @@ -21,6 +21,7 @@ class TrajectoryFollower:
generated_trajectories: Dict

def on_enable(self):
self._current_trajectory = None
self.last_difference = 0

self.left_follower = pf.followers.EncoderFollower(None)
Expand All @@ -43,18 +44,17 @@ def _cofigure_encoders(self):
self.right_follower.configureEncoder(self.r_encoder.get(), 360, self.WHEEL_DIAMETER)

def is_following(self, trajectory_name):
if self._current_trajectory == trajectory_name:
return True
return False
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
self._current_trajectory = None
return

l = self.left_follower.calculate(self.l_encoder.get())
r = self.right_follower.calculate(self.r_encoder.get())
left = self.left_follower.calculate(self.l_encoder.get())
right = self.right_follower.calculate(self.r_encoder.get())

gyro_heading = (
-self.navx.getAngle()
Expand All @@ -71,8 +71,8 @@ def execute(self):

self.last_difference = angle_difference

l = l + turn
r = r - turn
left += turn
right -= turn

# -1 is forward, so invert both values
self.drivetrain.tankDrive(-l, -r)
self.drivetrain.tankDrive(-left, -right)
46 changes: 20 additions & 26 deletions robot/physics.py
Original file line number Diff line number Diff line change
@@ -1,38 +1,32 @@
#
# See the notes for the other physics sample
#


import math
from pyfrc.physics import motor_cfgs, tankmodel
from pyfrc.physics.units import units
import math


class PhysicsEngine(object):
'''
Simulates a 4-wheel robot using Tank Drive joystick control
'''

"""
Simulates a 4-wheel robot using Tank Drive joystick control
"""

def __init__(self, physics_controller):
'''
:param physics_controller: `pyfrc.physics.core.Physics` object
"""
:param physics_controller: `pyfrc.physics.core.Physics` object
to communicate simulation effects to
'''
"""

self.physics_controller = physics_controller

# Change these parameters to fit your robot!

self.drivetrain = tankmodel.TankModel.theory(
motor_cfgs.MOTOR_CFG_CIM, # motor configuration
110*units.lbs, # robot mass
110 * units.lbs, # robot mass
12.75, # drivetrain gear ratio
2, # motors per side
24*units.inch, # robot wheelbase
25*units.inch, # robot width
30*units.inch, # robot length
6*units.inch # wheel diameter
24 * units.inch, # robot wheelbase
25 * units.inch, # robot width
30 * units.inch, # robot length
6 * units.inch # wheel diameter
)

self.kEncoder = 360 / (0.5 * math.pi)
Expand All @@ -41,14 +35,14 @@ def __init__(self, physics_controller):
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.
"""
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
'''
: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'][9]['value']
Expand All @@ -60,7 +54,7 @@ def update_sim(self, hal_data, now, tm_diff):

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
Expand Down
22 changes: 9 additions & 13 deletions robot/robot.py
Original file line number Diff line number Diff line change
Expand Up @@ -21,17 +21,16 @@ class Bot(magicbot.MagicRobot):

def createObjects(self):
# Joysticks

self.joystick = wpilib.Joystick(0)

# Drive motor controllers
# 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
# TODO: This is not in any way an ideal numbering system.
# The PWM ports should be redone to use the old layout above.
self.lf_motor = wpilib.Victor(9)
Expand All @@ -40,7 +39,7 @@ def createObjects(self):
self.rr_motor = wpilib.Victor(6)

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()
Expand Down Expand Up @@ -70,18 +69,15 @@ def createObjects(self):
def autonomous(self):
super().autonomous()

def disabledPeriodic(self): pass
def disabledInit(self): pass
def autonomousInit(self): pass
def autonomousPeriodic(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)

Expand Down
2 changes: 0 additions & 2 deletions robot/sim/config.json
Original file line number Diff line number Diff line change
@@ -1,13 +1,11 @@
{
"pyfrc": {

"robot": {
"w": 3,
"h": 2,
"starting_x": 1.8,
"starting_y": 14
},

"pwm": {
"9": "lf_motor",
"8": "lr_motor",
Expand Down
30 changes: 16 additions & 14 deletions robot/trajectory_generator.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,13 +4,13 @@
import wpilib


WHEELBASE_WIDTH = 2 # In feet
WHEELBASE_WIDTH = 2 # In feet
TRAJECTORY_DIRECTORY = 'trajectories'

PICKLE_FILE = os.path.join(os.path.dirname(__file__), TRAJECTORY_DIRECTORY, 'trajectories.pickle')

trajectories = {
"diagonal": [
pf.Waypoint(0, 0, 0), # Waypoints are relative to first, so start at 0, 0, 0
pf.Waypoint(0, 0, 0), # Waypoints are relative to first, so start at 0, 0, 0
pf.Waypoint(15, 5, 0)
],
"charge": [
Expand All @@ -19,34 +19,36 @@
]
}


def load_trajectories():
if wpilib.RobotBase.isSimulation():
generated_trajectories = _generate_trajectories()
_write_trajectories(generated_trajectories)
else:
with open(os.path.join(os.path.dirname(__file__) + os.sep + TRAJECTORY_DIRECTORY, 'trajectories.pickle'), 'rb') as f:
with open(PICKLE_FILE, 'rb') as f:
generated_trajectories = pickle.load(f)

return generated_trajectories


def _write_trajectories(trajectories):
pickle_file = os.path.join(os.path.dirname(__file__) + os.sep + TRAJECTORY_DIRECTORY, 'trajectories.pickle')
with open(pickle_file, 'wb') as f:
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=53.251, # set the units for distance to ft.
max_jerk=120
)[1] # The 0th element is just info
trajectory,
pf.FIT_HERMITE_CUBIC,
pf.SAMPLES_HIGH,
dt=0.02, # 20ms
max_velocity=10.903, # These are in ft/sec and
max_acceleration=53.251, # set the units for distance to ft.
max_jerk=120
)[1] # The 0th element is just info

modifier = pf.modifiers.TankModifier(generated_trajectory).modify(WHEELBASE_WIDTH)

Expand Down
2 changes: 1 addition & 1 deletion test.sh
Original file line number Diff line number Diff line change
Expand Up @@ -5,4 +5,4 @@ python3 robot/robot.py test
# E501 line too long
# E701 multiple statements on one line
# Currently, E701 takes issue with some type annotations. (PyCQA/pycodestyle#682)
pycodestyle . --ignore=E501,E701
pycodestyle robot/ --ignore=E501,E701

0 comments on commit b1bef71

Please sign in to comment.