def __init__(self, periodic, motor_system, **kwargs):
self.dt_s = kwargs.pop('dt_s', 0.0001)
self.nominal_voltage = kwargs.pop('nominal_voltage', 12.)
self.starting_position_rad = kwargs.pop('starting_position_rad', 0.)
self.spool_radius_m = kwargs.pop('spool_radius_m', inch_to_meter(0.5))
self.spur_radius_m = kwargs.pop('spur_radius_m', inch_to_meter(2))
self.spur_offset_rad = kwargs.pop('spur_offset_rad', math.radians(120))
self.theta_eqbm = kwargs.pop('theta_eqbm', math.radians(90))
self.x1_m = kwargs.pop('x1_m', inch_to_meter(0.5))
self.x2_m = kwargs.pop('x2_m', inch_to_meter(1.5))
self.intake_inertial = kwargs.pop('intake_inertial',
inch_to_meter(5)**2 * lbs_to_kg(5))
self.spring_constant = kwargs.pop('spring_constant', 50)
self.gearbox_efficiency = kwargs.pop('gearbox_efficiency', 0.65)
self.battery_resistance_ohms = kwargs.pop('battery_resistance_ohms',
0.015)
self.battery_voltage = kwargs.pop('battery_voltage', 12.7)
self.fuse_resistance_ohms = kwargs.pop('fuse_resistance_ohms', 0.002)
self.pid_sample_rate = kwargs.pop('pid_sample_rate_s', 0.05)
self.init = kwargs.pop('init', lambda *args, **kwargs: None)
assert 0 <= self.gearbox_efficiency <= 1
assert len(kwargs) == 0, 'Unknown parameters: ' + ', '.join(
kwargs.keys())
self.periodic = periodic
self.periodic_period = 0.02 # called on 20 ms intervals
self.motor_system = motor_system
self.ts = None
self.a_s = None
self.vs = None
self.xs = None
self.voltages = None
self.voltageps = None
self.currents = None
def __init__(self, periodic, motor_system, **kwargs):
self.dt_s = kwargs.pop('dt_s', 0.0001)
self.nominal_voltage = kwargs.pop('nominal_voltage', 12.)
self.starting_position_rad = kwargs.pop('starting_position_rad', 0.)
self.release_position_rad = kwargs.pop('release_position_rad',
math.radians(45))
self.end_mass_kg = kwargs.pop('end_mass_kg', lbs_to_kg(5.0))
self.arm_mass_kg = kwargs.pop('arm_mass_kg', lbs_to_kg(2.0))
self.arm_length_m = kwargs.pop('arm_length_m', inch_to_meter(12.))
self.damping = kwargs.pop('damping', 0.1) # kg/s
self.pid_sample_rate = kwargs.pop('pid_sample_rate_s', 0.05)
self.periodic = periodic
self.pid_periodic = kwargs.pop('pid_periodic', lambda state: 1)
self.periodic_period = 0.02 # called on 20 ms intervals
self.motor_system = motor_system
self.init = kwargs.pop('init', lambda *args, **kwargs: None)
assert len(kwargs) == 0, 'Unknown parameters: ' + ', '.join(
kwargs.keys())
self.ts = None
self.a_s = None
self.vs = None
self.thetas = None
self.voltages = None
self.currents = None
def __init__(self, periodic, motor_system, **kwargs):
self.dt_s = kwargs.pop('dt_s', 0.0001)
self.nominal_voltage = kwargs.pop('nominal_voltage', 12.)
self.starting_position_m = kwargs.pop('starting_position_m', 0.)
self.stage1_mass_kg = kwargs.pop('stage1_mass_kg', lbs_to_kg(5.0))
self.stage2_mass_kg = kwargs.pop('stage2_mass_kg', lbs_to_kg(5.0))
self.stage3_mass_kg = kwargs.pop('stage3_mass_kg', lbs_to_kg(20.0))
self.robot_mass_kg = kwargs.pop('robot_mass_kg', lbs_to_kg(154))
self.stage1_counterweighting_n = kwargs.pop(
'stage1_counterweighting_n', lbs_to_N(0.0))
self.stage2_counterweighting_n = kwargs.pop(
'stage2_counterweighting_n', lbs_to_N(0.0))
self.max_height_m = kwargs.pop('max_height_m', 2.0)
# assuming kinetic and max static are same
self.friction_force = kwargs.pop('friction_force_N', 0)
self.sprocket_radius_m = kwargs.pop('sprocket_radius_m',
inch_to_meter(2.0))
self.climb_noload_zone = kwargs.pop('climb_noload_zone', (0, 0))
self.hardstop_spring_constant = kwargs.pop('hardstop_spring_constant',
lbs_to_N(200) / 0.01)
self.gearbox_efficiency = kwargs.pop('gearbox_efficiency', 0.65)
self.battery_resistance_ohms = kwargs.pop('battery_resistance_ohms',
0.015)
self.pulls_down = kwargs.pop('pulls_down', True)
self.cascading = kwargs.pop('cascading', True)
self.battery_voltage = kwargs.pop('battery_voltage', 12.7)
self.fuse_resistance_ohms = kwargs.pop('fuse_resistance_ohms', 0.002)
self.pid_sample_rate = kwargs.pop('pid_sample_rate_s', 0.05)
self.pid_periodic = kwargs.pop('pid_periodic', lambda state: 1)
self.periodic_period = kwargs.pop('periodic_period',
0.02) # default on 20 ms intervals
self.init = kwargs.pop('init', lambda *args, **kwargs: None)
assert 0 <= self.gearbox_efficiency <= 1
assert len(kwargs) == 0, 'Unknown parameters: ' + ', '.join(
kwargs.keys())
self.periodic = periodic
self.motor_system = motor_system
self.ts = None
self.a_s = None
self.vs = None
self.xs = None
self.voltages = None
self.voltageps = None
self.currents = None
import math
from frc3223_azurite.conversions import inch_to_meter, g, meter_to_inch, radps_to_rpm
angle = math.radians(45)
arm_length_in = 25
base_height_in = 7
assert base_height_in + arm_length_in < 55
h_displ = inch_to_meter(72-arm_length_in)
v_displ = inch_to_meter(79 - (base_height_in + arm_length_in + (arm_length_in - 6.5) * math.sin(angle)))
print ('horizontal displacement: %s m' % h_displ)
print ('vertical displacement: %s m' % (v_displ))
# t = h_displ / (v cos(angle))
vx = math.sqrt(0.5 * g * h_displ ** 2 / (math.tan(angle) * h_displ - v_displ))
v = vx / math.cos(angle)
print ('linear velocity: %s m/s' % v)
w = v / inch_to_meter(arm_length_in)
print ('rotational velocity: %s rad/s (%s deg/s) (%s rpm)' % (w, math.degrees(w), radps_to_rpm(w)))
print ('time to travel: %s s' %( (angle + math.radians(90)) / w))
# post commit a518 this doesn't need to be implemented
return int((self.getTime() - hal_data['time']['program_start']) * 1000000)
if __name__ == '__main__':
from frc3223_azurite.motors import *
def init(state):
state.voltage_p = 0.0
def periodic(state):
state.voltage_p = 1.0
sim = ElevatorSimulation(
starting_position_m=inch_to_meter(0),
max_height_m=inch_to_meter(72),
sprocket_radius_m=inch_to_meter(2.12),
robot_mass_kg=lbs_to_kg(154),
stage1_mass_kg=lbs_to_kg(16.0),
stage2_mass_kg=lbs_to_kg(0.0),
stage3_mass_kg=lbs_to_kg(0.0),
stage1_counterweighting_n=lbs_to_N(.0),
stage2_counterweighting_n=lbs_to_N(0.0),
pulls_down = False,
init=init,
periodic=periodic,
pid_sample_rate_s=0.001,
gearbox_efficiency=0.85,
motor_system=MotorSystem(motor=bag, motor_count=4, gearing_ratio=50),
dt_s=0.0001,
def init(state):
state.voltage_p = 1.0
def periodic(state):
theta = state.theta_rad
err = math.radians(45) - theta
#p = 2 / math.radians(45)
#voltage = err * p
theta_deg = math.degrees(theta)
if theta_deg > 60:
voltage = -0.1
elif theta_deg > 50:
voltage = 0.0
elif theta_deg > 35:
voltage = 0.2
else:
voltage = 0.4
state.voltage_p = voltage
ArmSimulation(
dt_s=0.0001,
starting_position_rad=math.radians(0),
end_mass_kg=lbs_to_kg(5.0),
arm_mass_kg=lbs_to_kg(2.0),
arm_length_m=inch_to_meter(15),
nominal_voltage=12.,
periodic = periodic,
init = init,
motor_system=MotorSystem(motor=am9015, motor_count=1, gearing_ratio=64),
).run_sim()