class PhysicsEngine:
"""
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
"""
# array of (found, timestamp, angle)
target = ntproperty("/camera/target", (0.0, float("inf"), 0.0))
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")
targets = [
# right
VisionSim.Target(15, 13, 250, 0),
# middle
VisionSim.Target(16.5, 15.5, 295, 65),
# left
VisionSim.Target(15, 18, 0, 110),
]
self.vision = VisionSim(targets,
61.0,
1.5,
15,
15,
physics_controller=physics_controller)
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"][0]["value"]
r_motor = hal_data["pwm"][1]["value"]
speed, rotation = drivetrains.two_motor_drivetrain(l_motor, r_motor)
self.physics_controller.drive(speed, rotation, tm_diff)
x, y, angle = self.physics_controller.get_position()
data = self.vision.compute(now, x, y, angle)
if data is not None:
self.target = data[0][:3]
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
'''
# array of (found, timestamp, angle)
target = ntproperty('/camera/target', (0.0, float('inf'), 0.0))
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')
targets = [
# right
VisionSim.Target(15, 13, 250, 0),
# middle
VisionSim.Target(16.5, 15.5, 295, 65),
# left
VisionSim.Target(15, 18, 0, 110)
]
self.vision = VisionSim(targets, 61.0, 1.5, 15, 15,
physics_controller=physics_controller)
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'][0]['value']
r_motor = hal_data['pwm'][1]['value']
#global l_motor = 0.5
#global r_motor = 0.5
#import pdb; set_trace();
#print("hal_data", l_motor)
speed, rotation = drivetrains.two_motor_drivetrain(l_motor, r_motor)
self.physics_controller.drive(speed, rotation, tm_diff)
#print("update_sim left right forward rotate", l_motor, r_motor, speed, rotation)
x, y, angle = self.physics_controller.get_position()
#import pdb;pdb.set_trace()
data = self.vision.compute(now, x, y, angle)
if data is not None:
self.target = data[0][:3]
class PhysicsEngine:
def __init__(self, physicsController):
self.physicsController = physicsController
# NavX simulation
self.ahrs = None
def createAHRSSim():
self.ahrs = AHRSSim()
return self.ahrs
navx.AHRS.create_spi = createAHRSSim
self.physicsController.add_analog_gyro_channel(0)
config = configparser.ConfigParser()
filename = os.path.dirname(os.path.abspath(
inspect.getfile(inspect.currentframe()))) \
+ os.sep + "sim" + os.sep + "drivetrain.ini"
print("Reading robot data from", filename)
config.read(filename)
self.simulatedTalons = []
if 'talons' in config:
for key, value in config['talons'].items():
num = int(key.replace('talon', ''))
maxVel = float(value)
self.simulatedTalons.append(SimulatedTalon(num, maxVel))
if 'ds' in config:
ds = config['ds']
else:
ds = {}
location = int(ds.get('location', '1'))
team = 1 if (ds.get('team', 'red').lower() == 'blue') else 0
self.allianceStation = location - 1 + team * 3
if 'field' in config:
field = config['field']
else:
field = {}
self.visionX = float(field.get('visionx', '0'))
self.visionY = float(field.get('visiony', '0'))
self.visionAngleStart = float(field.get('visionanglestart', '90'))
self.visionAngleEnd = float(field.get('visionangleend', '270'))
# special function called by pyfrc when simulator starts
def initialize(self, hal_data):
self.visionTable = NetworkTables.getTable('limelight')
self.visionTable.putNumber('tv',
1) # some arbitrary default vision data
self.visionTable.putNumber('tx', 0)
self.visionTable.putNumber('ty', 0)
self.visionTable.putNumber('ts', 0)
self.visionTable.putNumber('ta', 5)
visionTarget = VisionSim.Target(self.visionX, self.visionY,
self.visionAngleStart,
self.visionAngleEnd)
self.visionSim = VisionSim([visionTarget], 60, 2, 50)
hal_data['alliance_station'] = self.allianceStation
# special function called by pyfrc to update the robot state
def update_sim(self, hal_data, time, elapsed):
global simulatedDrivetrain
for simTalon in self.simulatedTalons:
simTalon.update(hal_data)
if simulatedDrivetrain is not None:
robotMag, robotDir, robotTurn = simulatedDrivetrain.getRobotMovement(
)
xVel = robotMag * math.cos(robotDir)
yVel = robotMag * math.sin(robotDir)
self.physicsController.vector_drive(xVel, yVel, -robotTurn,
elapsed)
if self.ahrs != None:
self.ahrs.angle = math.degrees(self.physicsController.angle)
x, y, angle = self.physicsController.get_position()
visionData = self.visionSim.compute(time, x, y, angle)
if visionData is not None:
targetData = visionData[0]
self.visionTable.putNumber('tv', targetData[0])
if targetData[0] != 0:
self.visionTable.putNumber('tx', targetData[2])
else:
# DOESN'T mean no vision. vision just doesn't always update
pass
class PhysicsEngine(object):
"""
Simulates a 4-wheel mecanum robot using Tank Drive joystick control
"""
target = ntproperty("/camera/target", (0.0, 0.0, 0.0)) #angle and distance
def __init__(self, physics_controller):
"""
:param physics_controller: `pyfrc.physics.core.Physics` object
to communicate simulation effects to
"""
self.physics_controller = physics_controller
self.position = 0
self.physics_controller.add_device_gyro_channel("navxmxp_i2c_1_angle")
self.physics_controller.add_device_gyro_channel("navxmxp_spi_4_angle")
targets = [
#everything on the left side of the field
VisionSim.Target(18.5, 12.5, 90, 270), # front cargo
VisionSim.Target(0, 2, -90, 90), # pickup station
VisionSim.Target(22, 11.25, 180, 360), # cargo side 1
VisionSim.Target(23.75, 11.25, 180, 360), # cargo side 2
VisionSim.Target(25.5, 11.25, 180, 360), # cargo side 3
VisionSim.Target(17.5, 2, 90 - 29, 90 - 29 + 180), # rocket fromt
VisionSim.Target(21, 2, -90 + 29, -90 + 29 + 180), # rocket back
#everything on the left side of the field
VisionSim.Target(18.5, 27 - 12.5, 90, 270), # front cargo
VisionSim.Target(0, 27 - 2, -90, 90), # pickup station
VisionSim.Target(22, 27 - 11.25, 0, 180), # cargo side 1
VisionSim.Target(23.75, 27 - 11.25, 0, 180), # cargo side 2
VisionSim.Target(25.5, 27 - 11.25, 0, 180), # cargo side 3
VisionSim.Target(17.5, 27 - 2, 90 + 29,
90 + 29 + 180), # rocket fromt
VisionSim.Target(21, 27 - 2, -90 - 29,
-90 - 29 + 180), # rocket back
VisionSim.Target(18.5, 14.5, 90, 270), # front right cargo
]
self.vision = VisionSim(
targets,
61.0, #camera FOV
.1, #close limit (ft)
10, #far limit (ft)
15, #update rate (hz)
physics_controller=physics_controller)
#Parameters:
# targets – List of target positions (x, y) on field in feet
# view_angle_start – Center angle that the robot can ‘see’ the target from (in degrees)
# camera_fov – Field of view of camera (in degrees)
# view_dst_start – If the robot is closer than this, the target cannot be seen
# view_dst_end – If the robot is farther than this, the target cannot be seen
# data_frequency – How often the camera transmits new coordinates
# data_lag – How long it takes for the camera data to be processed and make it to the robot
# physics_controller – If set, will draw target information in UI
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"][3]["value"]
lf_motor = -hal_data["pwm"][0]["value"]
rf_motor = hal_data["pwm"][2]["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)
x, y, angle = self.physics_controller.get_position()
data = self.vision.compute(now, x, y, angle)
if data is not None:
self.target = [data[0][0], data[0][2], 12 * data[0][3]]
class PhysicsEngine(object):
"""
Your physics module must contain a class called ``PhysicsEngine``,
and it must implement the same functions as this class.
Alternatively, you can inherit from this object. However, that is
not required.
"""
# array of (found, timestamp, angle)
target = ntproperty("/camera/target", (0.0, float("inf"), 0.0))
def __init__(self, physics_controller):
"""
The constructor must take the following arguments:
:param physics_controller: The physics controller interface
:type physics_controller: :class:`.PhysicsInterface`
"""
self.physics_controller = physics_controller
self.position = 0
self.physics_controller.add_device_gyro_channel("navxmxp_spi_4_angle")
# Change these parameters to fit your robot!
bumper_width = 3.25 * units.inch
self.drivetrain = tankmodel.TankModel.theory(
motor_cfgs.MOTOR_CFG_CIM, # motor configuration
137 * units.lbs, # robot mass
1 / 75, # drivetrain gear ratio
2, # motors per side
35 * units.inch, # robot wheelbase
29 * units.inch + bumper_width * 2, # robot width
29 * units.inch + bumper_width * 2, # robot length
8 * units.centimeter, # wheel diameter
)
# Precompute the encoder constant
# -> encoder counts per revolution / wheel circumference
self.kEncoder = (4096) / (.08 * math.pi)
self.l_distance = 0
self.r_distance = 0
targets = [
# right
VisionSim.Target(15, 13, 250, 0),
# middle
VisionSim.Target(16.5, 15.5, 295, 65),
# left
VisionSim.Target(15, 18, 0, 110),
VisionSim.Target(10, 25, 250, 0),
]
self.vision = VisionSim(targets,
61.0,
1.5,
20,
15,
physics_controller=physics_controller)
def initialize(self, hal_data):
"""
Called with the hal_data dictionary before the robot has started
running. Some values may be overwritten when devices are
initialized... it's not consistent yet, sorry.
"""
pass
def update_sim(self, hal_data, now, tm_diff):
"""
Called when the simulation parameters for the program need to be
updated. This is mostly when ``wpilib.Timer.delay()`` is called.
:param hal_data: A giant dictionary that has all data about the robot. See
``hal-sim/hal_impl/data.py`` in robotpy-wpilib's repository
for more information on the contents of this dictionary.
:param now: The current time
:type now: float
:param tm_diff: The amount of time that has passed since the last
time that this function was called
:type tm_diff: float
"""
# Simulate the drivetrain
l_motor = hal_data["CAN"][1]
r_motor = hal_data["CAN"][2]
#print(l_motor['value'], r_motor['value'])
x, y, angle = self.drivetrain.get_distance(l_motor["value"],
r_motor["value"], tm_diff)
#print(x, y, angle)
self.physics_controller.distance_drive(x, y, angle)
data = self.vision.compute(now, x, y, 0)
#print("x, y, angle of robot",x,y,angle)
if data is not None:
self.target = data[0][:3]
# print("data: ",data)
# print("target: ",self.target)
# encoder increments speed mutiplied by the time by some constant
# -> must be an integer
l_speed = int(4096 * l_motor["value"] * tm_diff)
r_speed = int(4096 * r_motor["value"] * tm_diff)
# Update encoders
self.l_distance += self.drivetrain.l_velocity * tm_diff
self.r_distance += self.drivetrain.r_velocity * tm_diff
l_motor["quad_position"] = int(self.l_distance * self.kEncoder)
r_motor["quad_position"] = int(self.r_distance * self.kEncoder)
class PhysicsEngine:
"""
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
"""
# array of (found, timestamp, angle)
target = ntproperty("/camera/target", (0.0, float("inf"), 0.0))
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
targets = [
# right
VisionSimTarget(15, 13, 250, 0),
# middle
VisionSimTarget(16.5, 15.5, 295, 65),
# left
VisionSimTarget(15, 18, 0, 110),
]
self.vision = VisionSim(targets,
61.0,
1.5,
15,
15,
physics_controller=physics_controller)
# Simulate the drivetrain.
self.drivetrain = drivetrains.TwoMotorDrivetrain(
deadzone=drivetrains.linear_deadzone(0.1))
# Create the motors.
self.l_motor = PWMSim(1)
self.r_motor = PWMSim(2)
self.gyroSim = AnalogGyroSim(0)
self.system = LinearSystemId.identifyDrivetrainSystem(
1.98, 0.2, 1.5, 0.3)
self.drivesim = DifferentialDrivetrainSim(
self.system,
0.762,
DCMotor.CIM(2),
8,
0.0508,
)
self.leftEncoderSim = EncoderSim.createForChannel(0)
self.rightEncoderSim = EncoderSim.createForChannel(2)
def update_sim(self, 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
"""
l_speed = self.l_motor.getSpeed()
r_speed = self.r_motor.getSpeed()
voltage = RobotController.getInputVoltage()
self.drivesim.setInputs(l_speed * voltage, r_speed * voltage)
self.drivesim.update(tm_diff)
self.leftEncoderSim.setDistance(self.drivesim.getLeftPosition() *
39.37)
self.leftEncoderSim.setRate(self.drivesim.getLeftVelocity() * 39.37)
self.rightEncoderSim.setDistance(self.drivesim.getRightPosition() *
39.37)
self.rightEncoderSim.setRate(self.drivesim.getRightVelocity() * 39.37)
self.physics_controller.field.setRobotPose(self.drivesim.getPose())
pose = self.drivesim.getPose()
currentTranslation = pose.translation()
currentRotation = pose.rotation()
x = currentTranslation.X()
y = currentTranslation.Y()
angle = currentRotation.degrees()
data = self.vision.compute(now, x, y, angle)
if data is not None:
self.target = data[0][:3]
class PhysicsEngine:
def __init__(self, physicsController):
self.physicsController = physicsController
# NavX simulation
self.physicsController.add_analog_gyro_channel(0)
config = configparser.ConfigParser()
filename = os.path.dirname(
os.path.abspath(inspect.getfile(inspect.currentframe()))
) + os.sep + "sim" + os.sep + "drivetrain.ini"
print("Reading robot data from", filename)
config.read(filename)
if not 'physics' in config:
print("Please use the [physics] header in drivetrain.ini")
exit()
physics = config['physics']
# get() allows fallback values if the key isn't in the config file
self.xLen = float(physics.get('xlen', '2'))
self.yLen = float(physics.get('ylen', '3'))
self.speed = float(physics.get('speed', '6'))
self.drivetrain = physics.get('drivetrain', 'four')
self.flMotor = SimulatedTalon(physics.get('canfl', '0'))
self.frMotor = SimulatedTalon(physics.get('canfr', '0'))
self.blMotor = SimulatedTalon(physics.get('canbl', '0'))
self.brMotor = SimulatedTalon(physics.get('canbr', '0'))
self.maxVel = int(physics.get('maxvel', '8000'))
ds = config['ds']
location = int(ds.get('location', '1'))
team = 1 if (ds.get('team', 'red').lower() == 'blue') else 0
self.allianceStation = location - 1 + team * 3
field = config['field']
self.visionX = float(field.get('visionx', '0'))
self.visionY = float(field.get('visiony', '0'))
self.visionAngleStart = float(field.get('visionanglestart', '90'))
self.visionAngleEnd = float(field.get('visionangleend', '270'))
def initialize(self, hal_data):
self.visionTable = NetworkTables.getTable('limelight')
self.visionTable.putNumber('tv', 1)
self.visionTable.putNumber('tx', 0)
self.visionTable.putNumber('ty', 0)
self.visionTable.putNumber('ts', 0)
self.visionTable.putNumber('ta', 5)
visionTarget = VisionSim.Target(self.visionX, self.visionY,
self.visionAngleStart,
self.visionAngleEnd)
self.visionSim = VisionSim([visionTarget], 60, 2, 50)
hal_data['alliance_station'] = self.allianceStation
def update_sim(self, data, time, elapsed):
fl = self.flMotor.getSpeed(data, self.maxVel)
fr = self.frMotor.getSpeed(data, self.maxVel)
bl = self.blMotor.getSpeed(data, self.maxVel)
br = self.brMotor.getSpeed(data, self.maxVel)
if self.drivetrain == "mecanum":
vx, vy, vw = drivetrains.mecanum_drivetrain(
bl, br, fl, fr, self.xLen, self.yLen, self.speed)
self.physicsController.vector_drive(vx, vy, vw, elapsed)
elif self.drivetrain == "four":
speed, rot = drivetrains.four_motor_drivetrain(
bl, br, fl, fr, self.xLen, self.speed)
self.physicsController.drive(speed, rot, elapsed)
elif self.drivetrain == "two":
speed, rot = drivetrains.two_motor_drivetrain(
fl, fr, self.xLen, self.speed)
self.physicsController.drive(speed, rot, elapsed)
# https://github.com/robotpy/robotpy-wpilib/issues/291
data['analog_gyro'][0]['angle'] = math.degrees(
self.physicsController.angle)
x, y, angle = self.physicsController.get_position()
visionData = self.visionSim.compute(time, x, y, angle)
if visionData is not None:
targetData = visionData[0]
self.visionTable.putNumber('tv', targetData[0])
if targetData[0] != 0:
self.visionTable.putNumber('tx', targetData[2])
else:
# DOESN'T mean no vision. vision just doesn't always update
pass
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.ft_per_sec = 5
self.wheel_circumference = 18.8
self.physics_controller.add_device_gyro_channel('adxrs450_spi_0_angle')
if VisionSim is not None:
targets = [
# right
VisionSim.Target(16, 12, 250, 20), # angle is 122.23
# middle
VisionSim.Target(18.5, 16, 295, 65), # angle is 180
# left
VisionSim.Target(16, 20, 320, 110), # angle is -142
]
self.vision = VisionSim(targets,
61.0,
1.5,
15,
15,
physics_controller=self.physics_controller)
else:
self.vision = None
@property
def nt(self):
try:
return self._nt
except AttributeError:
self._nt = NetworkTables.getTable('/')
return self._nt
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'][0]['value']
r_motor = -hal_data['pwm'][1]['value']
speed, rotation = drivetrains.two_motor_drivetrain(
l_motor, r_motor, speed=self.ft_per_sec)
self.physics_controller.drive(speed, rotation, tm_diff)
# Inches we traveled
distance_inches = 12.0 * speed * tm_diff
# Use that to give a rough approximation of encoder values.. not
# accurate for turns, but we don't need that
# -> encoder = distance / (wheel_circumference / 360.0)
hal_data['encoder'][0]['count'] += int(
distance_inches / (self.wheel_circumference / 360.0))
if self.vision:
x, y, angle = self.physics_controller.get_position()
#data = None
data = self.vision.compute(now, x, y, angle)
if data is not None:
self.nt.putNumberArray('/camera/target', data[0][:3])
distance = self.vision.get_immediate_distance()
if distance is None:
distance = 5.0 * 0.3
hal_data['analog_in'][0]['avg_voltage'] = max(min(distance * 0.3, 5.0),
0.0)
