-
Notifications
You must be signed in to change notification settings - Fork 0
/
physics.py
51 lines (47 loc) · 1.86 KB
/
physics.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
import sys
import os
import json
import math
from pyfrc.physics import motor_cfgs, tankmodel
from pyfrc.physics.units import units
class PhysicsEngine:
def __init__(self, physics_controller):
"""
:param physics_controller: `pyfrc.physics.core.Physics` object
to communicate simulation effects to
"""
self.physics_controller = physics_controller
# Load ports
with open(sys.path[0] +
("/../" if os.getcwd()[-5:-1] == "test" else "/") +
"config/ports.json") as f:
self.ports = json.load(f)
# Drivetrain
self.drivetrain = tankmodel.TankModel.theory(
motor_cfgs.MOTOR_CFG_CIM,
robot_mass=110 * units.lbs,
gearing=10.71,
nmotors=2,
x_wheelbase=2.0 * units.feet,
wheel_diameter=8 * units.inch)
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
"""
# Get motors
fl_motor = hal_data["CAN"][self.ports["drive"]["front_left"]]
fr_motor = hal_data["CAN"][self.ports["drive"]["front_right"]]
# Simulate drivetrain
x, y, angle = self.drivetrain.get_distance(-fl_motor["value"],
fr_motor["value"], tm_diff)
self.physics_controller.distance_drive(x, y, angle)
# Update encoders
fl_motor["quad_position"] = int(
self.drivetrain.l_position * 6144 / math.pi)
fr_motor["quad_position"] = int(
self.drivetrain.r_position * 6144 / math.pi)
# Update navx