コード例 #1
0
ファイル: intake.py プロジェクト: adsnaider/Robotics-Project
def main(argv):
  argv = FLAGS(argv)
  glog.init()

  scenario_plotter = ScenarioPlotter()

  intake = Intake()
  intake_controller = IntegralIntake()
  observer_intake = IntegralIntake()

  # Test moving the intake with constant separation.
  initial_X = numpy.matrix([[0.0], [0.0]])
  R = numpy.matrix([[numpy.pi/2.0], [0.0], [0.0]])
  scenario_plotter.run_test(intake, end_goal=R,
                            controller_intake=intake_controller,
                            observer_intake=observer_intake, iterations=200)

  if FLAGS.plot:
    scenario_plotter.Plot()

  # Write the generated constants out to a file.
  if len(argv) != 5:
    glog.fatal('Expected .h file name and .cc file name for the intake and integral intake.')
  else:
    namespaces = ['y2016_bot3', 'control_loops', 'intake']
    intake = Intake("Intake")
    loop_writer = control_loop.ControlLoopWriter('Intake', [intake],
                                                 namespaces=namespaces)
    loop_writer.Write(argv[1], argv[2])

    integral_intake = IntegralIntake("IntegralIntake")
    integral_loop_writer = control_loop.ControlLoopWriter("IntegralIntake", [integral_intake],
                                                          namespaces=namespaces)
    integral_loop_writer.Write(argv[3], argv[4])
コード例 #2
0
ファイル: init.py プロジェクト: lxq2537664558/GGSvr
def main():

    # 初始化底层库
    # 配置文件
    gconfig.init()

    # 日志
    glog.init()

    # 网络
    gnet.init(gconfig.SVR_MAIN_IP, gconfig.SVR_MAIN_PORT)
    gnet.def_sub_server(gconfig.SVR_SUB_NAME)

    # 数据库
    #gdb.init()

    # mgr
    #account_mgr.init()
    #player_mgr.init()

    # 游戏逻辑
    #import svr_main
    #svr_main.init()
    # 测试 oche
    import oche; oche.init()
    # 测试 client-svr_main-svr_sub
    import svr_test_main; svr_test_main.init()
    

    # 开始服务
    gnet.start_loop()
コード例 #3
0
def main(argv):
    argv = FLAGS(argv)
    glog.init()

    if FLAGS.plot:
        drivetrain.PlotDrivetrainMotions(kDrivetrain)
    elif len(argv) != 5:
        print("Expected .h file name and .cc file name")
    else:
        # Write the generated constants out to a file.
        drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'o2017', kDrivetrain)
コード例 #4
0
ファイル: arm.py プロジェクト: adsnaider/Robotics-Project
def main(argv):
  argv = FLAGS(argv)
  glog.init()

  scenario_plotter = ScenarioPlotter()

  J_accelerating = 18
  J_decelerating = 7

  arm = Arm(name='AcceleratingArm', J=J_accelerating)
  arm_integral_controller = IntegralArm(
      name='AcceleratingIntegralArm', J=J_accelerating)
  arm_observer = IntegralArm()

  # Test moving the shoulder with constant separation.
  initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0], [0.0], [0.0]])
  R = numpy.matrix([[numpy.pi / 2.0],
                    [0.0],
                    [0.0], #[numpy.pi / 2.0],
                    [0.0],
                    [0.0],
                    [0.0]])
  arm.X = initial_X[0:4, 0]
  arm_observer.X = initial_X

  scenario_plotter.run_test(arm=arm,
                            end_goal=R,
                            iterations=300,
                            controller=arm_integral_controller,
                            observer=arm_observer)

  if len(argv) != 5:
    glog.fatal('Expected .h file name and .cc file name for the wrist and integral wrist.')
  else:
    namespaces = ['y2016', 'control_loops', 'superstructure']
    decelerating_arm = Arm(name='DeceleratingArm', J=J_decelerating)
    loop_writer = control_loop.ControlLoopWriter(
        'Arm', [arm, decelerating_arm], namespaces=namespaces)
    loop_writer.Write(argv[1], argv[2])

    decelerating_integral_arm_controller = IntegralArm(
        name='DeceleratingIntegralArm', J=J_decelerating)

    integral_loop_writer = control_loop.ControlLoopWriter(
        'IntegralArm',
        [arm_integral_controller, decelerating_integral_arm_controller],
        namespaces=namespaces)
    integral_loop_writer.AddConstant(control_loop.Constant("kV_shoulder", "%f",
          arm_integral_controller.shoulder_Kv))
    integral_loop_writer.Write(argv[3], argv[4])

  if FLAGS.plot:
    scenario_plotter.Plot()
コード例 #5
0
def main(argv):
    argv = FLAGS(argv)
    glog.init()

    if FLAGS.plot:
        drivetrain.PlotDrivetrainMotions(kDrivetrain)
    elif len(argv) != 5:
        print("Expected .h file name and .cc file name")
    else:
        # Write the generated constants out to a file.
        namespaces = [
            'third_party', 'frc971', 'control_loops', 'drivetrain', 'y2016'
        ]
        drivetrain.WriteDrivetrainFullName(argv[1:3], argv[3:5], namespaces,
                                           namespaces, kDrivetrain)
コード例 #6
0
def main(argv):
    argv = FLAGS(argv)
    glog.init()

    estimator = DownEstimator()

    if FLAGS.plot:
        real_angles = [0]
        real_velocities = [0]
        estimated_angles = [0]
        estimated_velocities = [0]
        for _ in xrange(100):
            estimator.Predict(0)
            estimator.Update(numpy.sqrt(2) / 2.0, numpy.sqrt(2) / 2.0, 0, 0)
            real_angles.append(math.pi / 2)
            real_velocities.append(0)
            estimated_angles.append(estimator.X_hat[0, 0])
            estimated_velocities.append(estimator.X_hat[1, 0])
        angle = math.pi / 2
        velocity = 1
        for i in xrange(100):
            measured_velocity = velocity + (random.random() -
                                            0.5) * 0.01 + 0.05
            estimator.Predict(measured_velocity)
            estimator.Update(
                math.sin(angle) + (random.random() - 0.5) * 0.02, 0,
                math.cos(angle) + (random.random() - 0.5) * 0.02,
                measured_velocity)
            real_angles.append(angle)
            real_velocities.append(velocity)
            estimated_angles.append(estimator.X_hat[0, 0])
            estimated_velocities.append(estimator.X_hat[1, 0])
            angle += velocity * estimator.dt
        pylab.plot(range(len(real_angles)), real_angles)
        pylab.plot(range(len(real_velocities)), real_velocities)
        pylab.plot(range(len(estimated_angles)), estimated_angles)
        pylab.plot(range(len(estimated_velocities)), estimated_velocities)
        pylab.show()

    if len(argv) != 3:
        print "Expected .h file name and .cc file name"
    else:
        namespaces = ['frc971', 'control_loops', 'drivetrain']
        kf_loop_writer = control_loop.ControlLoopWriter("DownEstimator",
                                                        [estimator],
                                                        namespaces=namespaces)
        kf_loop_writer.Write(argv[1], argv[2])
コード例 #7
0
def main(argv):
  argv = FLAGS(argv)
  glog.init()

  estimator = DownEstimator()

  if FLAGS.plot:
    real_angles = [0]
    real_velocities = [0]
    estimated_angles = [0]
    estimated_velocities = [0]
    for _ in xrange(100):
      estimator.Predict(0)
      estimator.Update(numpy.sqrt(2) / 2.0, numpy.sqrt(2) / 2.0, 0, 0)
      real_angles.append(math.pi / 2)
      real_velocities.append(0)
      estimated_angles.append(estimator.X_hat[0, 0])
      estimated_velocities.append(estimator.X_hat[1, 0])
    angle = math.pi / 2
    velocity = 1
    for i in xrange(100):
      measured_velocity = velocity + (random.random() - 0.5) * 0.01 + 0.05
      estimator.Predict(measured_velocity)
      estimator.Update(math.sin(angle) + (random.random() - 0.5) * 0.02, 0,
                       math.cos(angle) + (random.random() - 0.5) * 0.02,
                       measured_velocity)
      real_angles.append(angle)
      real_velocities.append(velocity)
      estimated_angles.append(estimator.X_hat[0, 0])
      estimated_velocities.append(estimator.X_hat[1, 0])
      angle += velocity * estimator.dt
    pylab.plot(range(len(real_angles)), real_angles)
    pylab.plot(range(len(real_velocities)), real_velocities)
    pylab.plot(range(len(estimated_angles)), estimated_angles)
    pylab.plot(range(len(estimated_velocities)), estimated_velocities)
    pylab.show()

  if len(argv) != 3:
    print "Expected .h file name and .cc file name"
  else:
    namespaces = ['frc971', 'control_loops', 'drivetrain']
    kf_loop_writer = control_loop.ControlLoopWriter(
        "DownEstimator", [estimator],
        namespaces = namespaces)
    kf_loop_writer.Write(argv[1], argv[2])
コード例 #8
0
def main():
    """
    Main function to initialize logging and spin up the server.

    After the server is started we have to just sleep forever. There is no Wait() call like there
    is in gRPC libraries for other languages.
    """

    log.init()
    server = grpc.server(futures.ThreadPoolExecutor(max_workers=5))
    agent_pb2_grpc.add_RemoteAgentServicer_to_server(RandomAgentServicer(), server)
    server.add_insecure_port('[::]:50051')
    server.start()
    try:
        while True:
            time.sleep(60 * 60 * 24)
    except KeyboardInterrupt:
        server.stop(0)
コード例 #9
0
ファイル: init_sub.py プロジェクト: coollen/GGSvr
def main():

    # 初始化底层库
    # 配置文件
    gconfig.init()

    # 日志
    glog.init()

    # 网络
    gnet.init_sub_server(gconfig.SVR_MAIN_IP, gconfig.SVR_MAIN_PORT, gconfig.SVR_SUB_NAME, gconfig.SVR_SUB_ID)

    # 数据库
    #gdb.init()

    # 测试 client-svr_main-svr_sub
    import svr_test_sub
    svr_test_sub.init()

    # 开始服务
    gnet.start_loop()
コード例 #10
0
def main():

    # 初始化底层库
    # 配置文件
    gconfig.init()

    # 日志
    glog.init()

    # 网络
    gnet.init_sub_server(gconfig.SVR_MAIN_IP, gconfig.SVR_MAIN_PORT,
                         gconfig.SVR_SUB_NAME, gconfig.SVR_SUB_ID)

    # 数据库
    #gdb.init()

    # 测试 client-svr_main-svr_sub
    import svr_test_sub
    svr_test_sub.init()

    # 开始服务
    gnet.start_loop()
コード例 #11
0
ファイル: init.py プロジェクト: 253627764/GGSvr
def main():

    # 初始化底层库
    # 配置文件
    gconfig.init()

    # 日志
    glog.init()

    # 网络
    gnet.init()

    # 数据库
    #gdb.init()

    # mgr
    #account_mgr.init()
    #player_mgr.init()

    # 游戏逻辑
    game.init()

    # 开始服务
    gnet.start_loop()
コード例 #12
0
ファイル: init.py プロジェクト: 253627764/GGSvr
def main():

    # 初始化底层库
    # 配置文件
    gconfig.init()

    # 日志
    glog.init()

    # 网络
    gnet.init()

    # 数据库
    #gdb.init()

    # mgr
    #account_mgr.init()
    #player_mgr.init()

    # 游戏逻辑
    game.init()

    # 开始服务
    gnet.start_loop()
コード例 #13
0
    right_gear_plot.append((vdrivetrain.right_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)

    fwd_velocity = (vdrivetrain.X[1, 0] + vdrivetrain.X[0, 0]) / 2
    turn_velocity = (vdrivetrain.X[1, 0] - vdrivetrain.X[0, 0])
    if abs(fwd_velocity) < 0.0000001:
      radius_plot.append(turn_velocity)
    else:
      radius_plot.append(turn_velocity / fwd_velocity)

  # TODO(austin):
  # Shifting compensation.

  # Tighten the turn.
  # Closed loop drive.

  pylab.plot(t_plot, vl_plot, label='left velocity')
  pylab.plot(t_plot, vr_plot, label='right velocity')
  pylab.plot(t_plot, ul_plot, label='left voltage')
  pylab.plot(t_plot, ur_plot, label='right voltage')
  pylab.plot(t_plot, radius_plot, label='radius')
  pylab.plot(t_plot, left_gear_plot, label='left gear high')
  pylab.plot(t_plot, right_gear_plot, label='right gear high')
  pylab.legend()
  pylab.show()
  return 0

if __name__ == '__main__':
  argv = FLAGS(sys.argv)
  glog.init()
  sys.exit(main(argv))
コード例 #14
0
#!/usr/bin/env python
#coding=utf-8
__author__ = 'yanziang'

import glog as log
import gflags
import logging

FLAGS = gflags.FLAGS
FLAGS.verbosity = logging.DEBUG


def try_log():
    log.debug('2333happy deubgging....')
    log.info('it works')
    log.warn('something not ideal')
    log.error('something went wrong')
    log.fatal('AAAAAAAAAAA!')


if __name__ == '__main__':
    log.init()
    try_log()
コード例 #15
0
def main(argv):
  argv = FLAGS(argv)
  glog.init()

  # Simulate the response of the system to a step input.
  drivetrain = Drivetrain(left_low=False, right_low=False)
  simulated_left = []
  simulated_right = []
  for _ in xrange(100):
    drivetrain.Update(numpy.matrix([[12.0], [12.0]]))
    simulated_left.append(drivetrain.X[0, 0])
    simulated_right.append(drivetrain.X[2, 0])

  if FLAGS.plot:
    pylab.plot(range(100), simulated_left)
    pylab.plot(range(100), simulated_right)
    pylab.suptitle('Acceleration Test')
    pylab.show()

  # Simulate forwards motion.
  drivetrain = Drivetrain(left_low=False, right_low=False)
  close_loop_left = []
  close_loop_right = []
  left_power = []
  right_power = []
  R = numpy.matrix([[1.0], [0.0], [1.0], [0.0]])
  for _ in xrange(300):
    U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
                   drivetrain.U_min, drivetrain.U_max)
    drivetrain.UpdateObserver(U)
    drivetrain.Update(U)
    close_loop_left.append(drivetrain.X[0, 0])
    close_loop_right.append(drivetrain.X[2, 0])
    left_power.append(U[0, 0])
    right_power.append(U[1, 0])

  if FLAGS.plot:
    pylab.plot(range(300), close_loop_left, label='left position')
    pylab.plot(range(300), close_loop_right, label='right position')
    pylab.plot(range(300), left_power, label='left power')
    pylab.plot(range(300), right_power, label='right power')
    pylab.suptitle('Linear Move')
    pylab.legend()
    pylab.show()

  # Try turning in place
  drivetrain = Drivetrain()
  close_loop_left = []
  close_loop_right = []
  R = numpy.matrix([[-1.0], [0.0], [1.0], [0.0]])
  for _ in xrange(100):
    U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
                   drivetrain.U_min, drivetrain.U_max)
    drivetrain.UpdateObserver(U)
    drivetrain.Update(U)
    close_loop_left.append(drivetrain.X[0, 0])
    close_loop_right.append(drivetrain.X[2, 0])

  if FLAGS.plot:
    pylab.plot(range(100), close_loop_left)
    pylab.plot(range(100), close_loop_right)
    pylab.suptitle('Angular Move')
    pylab.show()

  # Try turning just one side.
  drivetrain = Drivetrain()
  close_loop_left = []
  close_loop_right = []
  R = numpy.matrix([[0.0], [0.0], [1.0], [0.0]])
  for _ in xrange(100):
    U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
                   drivetrain.U_min, drivetrain.U_max)
    drivetrain.UpdateObserver(U)
    drivetrain.Update(U)
    close_loop_left.append(drivetrain.X[0, 0])
    close_loop_right.append(drivetrain.X[2, 0])

  if FLAGS.plot:
    pylab.plot(range(100), close_loop_left)
    pylab.plot(range(100), close_loop_right)
    pylab.suptitle('Pivot')
    pylab.show()

  # Write the generated constants out to a file.
  drivetrain_low_low = Drivetrain(
      name="DrivetrainLowLow", left_low=True, right_low=True)
  drivetrain_low_high = Drivetrain(
      name="DrivetrainLowHigh", left_low=True, right_low=False)
  drivetrain_high_low = Drivetrain(
      name="DrivetrainHighLow", left_low=False, right_low=True)
  drivetrain_high_high = Drivetrain(
      name="DrivetrainHighHigh", left_low=False, right_low=False)

  kf_drivetrain_low_low = KFDrivetrain(
      name="KFDrivetrainLowLow", left_low=True, right_low=True)
  kf_drivetrain_low_high = KFDrivetrain(
      name="KFDrivetrainLowHigh", left_low=True, right_low=False)
  kf_drivetrain_high_low = KFDrivetrain(
      name="KFDrivetrainHighLow", left_low=False, right_low=True)
  kf_drivetrain_high_high = KFDrivetrain(
      name="KFDrivetrainHighHigh", left_low=False, right_low=False)

  if len(argv) != 5:
    print "Expected .h file name and .cc file name"
  else:
    namespaces = ['y2016_bot3', 'control_loops', 'drivetrain']
    dog_loop_writer = control_loop.ControlLoopWriter(
        "Drivetrain", [drivetrain_low_low, drivetrain_low_high,
                       drivetrain_high_low, drivetrain_high_high],
        namespaces = namespaces)
    dog_loop_writer.AddConstant(control_loop.Constant("kDt", "%f",
          drivetrain_low_low.dt))
    dog_loop_writer.AddConstant(control_loop.Constant("kStallTorque", "%f",
          drivetrain_low_low.stall_torque))
    dog_loop_writer.AddConstant(control_loop.Constant("kStallCurrent", "%f",
          drivetrain_low_low.stall_current))
    dog_loop_writer.AddConstant(control_loop.Constant("kFreeSpeedRPM", "%f",
          drivetrain_low_low.free_speed))
    dog_loop_writer.AddConstant(control_loop.Constant("kFreeCurrent", "%f",
          drivetrain_low_low.free_current))
    dog_loop_writer.AddConstant(control_loop.Constant("kJ", "%f",
          drivetrain_low_low.J))
    dog_loop_writer.AddConstant(control_loop.Constant("kMass", "%f",
          drivetrain_low_low.m))
    dog_loop_writer.AddConstant(control_loop.Constant("kRobotRadius", "%f",
          drivetrain_low_low.rb))
    dog_loop_writer.AddConstant(control_loop.Constant("kWheelRadius", "%f",
          drivetrain_low_low.r))
    dog_loop_writer.AddConstant(control_loop.Constant("kR", "%f",
          drivetrain_low_low.resistance))
    dog_loop_writer.AddConstant(control_loop.Constant("kV", "%f",
          drivetrain_low_low.Kv))
    dog_loop_writer.AddConstant(control_loop.Constant("kT", "%f",
          drivetrain_low_low.Kt))
    dog_loop_writer.AddConstant(control_loop.Constant("kLowGearRatio", "%f",
          drivetrain_low_low.G_low))
    dog_loop_writer.AddConstant(control_loop.Constant("kHighGearRatio", "%f",
          drivetrain_high_high.G_high))

    dog_loop_writer.Write(argv[1], argv[2])

    kf_loop_writer = control_loop.ControlLoopWriter(
        "KFDrivetrain", [kf_drivetrain_low_low, kf_drivetrain_low_high,
                         kf_drivetrain_high_low, kf_drivetrain_high_high],
        namespaces = namespaces)
    kf_loop_writer.Write(argv[3], argv[4])
コード例 #16
0
from testbench.subsystems.drivetrain.python import drivetrain
from third_party.frc971.control_loops.python import polydrivetrain

import gflags
import glog

__author__ = 'Austin Schuh ([email protected])'

FLAGS = gflags.FLAGS

try:
    gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
    pass


def main(argv):
    if FLAGS.plot:
        polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
    elif len(argv) != 5:
        glog.fatal('Expected .h file name and .cc file name')
    else:
        polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], 'testbench',
                                           drivetrain.kDrivetrain)


if __name__ == '__main__':
    argv = FLAGS(sys.argv)
    glog.init()
    sys.exit(main(argv))
コード例 #17
0
def main(argv):
  argv = FLAGS(argv)
  glog.init()

  # Simulate the response of the system to a step input.
  drivetrain = Drivetrain(left_low=False, right_low=False)
  simulated_left = []
  simulated_right = []
  for _ in xrange(100):
    drivetrain.Update(numpy.matrix([[12.0], [12.0]]))
    simulated_left.append(drivetrain.X[0, 0])
    simulated_right.append(drivetrain.X[2, 0])

  if FLAGS.plot:
    pylab.plot(range(100), simulated_left)
    pylab.plot(range(100), simulated_right)
    pylab.suptitle('Acceleration Test')
    pylab.show()

  # Simulate forwards motion.
  drivetrain = Drivetrain(left_low=False, right_low=False)
  close_loop_left = []
  close_loop_right = []
  left_power = []
  right_power = []
  R = numpy.matrix([[1.0], [0.0], [1.0], [0.0]])
  for _ in xrange(300):
    U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
                   drivetrain.U_min, drivetrain.U_max)
    drivetrain.UpdateObserver(U)
    drivetrain.Update(U)
    close_loop_left.append(drivetrain.X[0, 0])
    close_loop_right.append(drivetrain.X[2, 0])
    left_power.append(U[0, 0])
    right_power.append(U[1, 0])

  if FLAGS.plot:
    pylab.plot(range(300), close_loop_left, label='left position')
    pylab.plot(range(300), close_loop_right, label='right position')
    pylab.plot(range(300), left_power, label='left power')
    pylab.plot(range(300), right_power, label='right power')
    pylab.suptitle('Linear Move')
    pylab.legend()
    pylab.show()

  # Try turning in place
  drivetrain = Drivetrain()
  close_loop_left = []
  close_loop_right = []
  R = numpy.matrix([[-1.0], [0.0], [1.0], [0.0]])
  for _ in xrange(100):
    U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
                   drivetrain.U_min, drivetrain.U_max)
    drivetrain.UpdateObserver(U)
    drivetrain.Update(U)
    close_loop_left.append(drivetrain.X[0, 0])
    close_loop_right.append(drivetrain.X[2, 0])

  if FLAGS.plot:
    pylab.plot(range(100), close_loop_left)
    pylab.plot(range(100), close_loop_right)
    pylab.suptitle('Angular Move')
    pylab.show()

  # Try turning just one side.
  drivetrain = Drivetrain()
  close_loop_left = []
  close_loop_right = []
  R = numpy.matrix([[0.0], [0.0], [1.0], [0.0]])
  for _ in xrange(100):
    U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
                   drivetrain.U_min, drivetrain.U_max)
    drivetrain.UpdateObserver(U)
    drivetrain.Update(U)
    close_loop_left.append(drivetrain.X[0, 0])
    close_loop_right.append(drivetrain.X[2, 0])

  if FLAGS.plot:
    pylab.plot(range(100), close_loop_left)
    pylab.plot(range(100), close_loop_right)
    pylab.suptitle('Pivot')
    pylab.show()

  # Write the generated constants out to a file.
  drivetrain_low_low = Drivetrain(
      name="DrivetrainLowLow", left_low=True, right_low=True)
  drivetrain_low_high = Drivetrain(
      name="DrivetrainLowHigh", left_low=True, right_low=False)
  drivetrain_high_low = Drivetrain(
      name="DrivetrainHighLow", left_low=False, right_low=True)
  drivetrain_high_high = Drivetrain(
      name="DrivetrainHighHigh", left_low=False, right_low=False)

  kf_drivetrain_low_low = KFDrivetrain(
      name="KFDrivetrainLowLow", left_low=True, right_low=True)
  kf_drivetrain_low_high = KFDrivetrain(
      name="KFDrivetrainLowHigh", left_low=True, right_low=False)
  kf_drivetrain_high_low = KFDrivetrain(
      name="KFDrivetrainHighLow", left_low=False, right_low=True)
  kf_drivetrain_high_high = KFDrivetrain(
      name="KFDrivetrainHighHigh", left_low=False, right_low=False)

  if len(argv) != 5:
    print "Expected .h file name and .cc file name"
  else:
    namespaces = ['testbench', 'subsystems', 'drivetrain']
    dog_loop_writer = control_loop.ControlLoopWriter(
        "Drivetrain", [drivetrain_low_low, drivetrain_low_high,
                       drivetrain_high_low, drivetrain_high_high],
        namespaces = namespaces)
    dog_loop_writer.AddConstant(control_loop.Constant("kDt", "%f",
          drivetrain_low_low.dt))
    dog_loop_writer.AddConstant(control_loop.Constant("kStallTorque", "%f",
          drivetrain_low_low.stall_torque))
    dog_loop_writer.AddConstant(control_loop.Constant("kStallCurrent", "%f",
          drivetrain_low_low.stall_current))
    dog_loop_writer.AddConstant(control_loop.Constant("kFreeSpeedRPM", "%f",
          drivetrain_low_low.free_speed))
    dog_loop_writer.AddConstant(control_loop.Constant("kFreeCurrent", "%f",
          drivetrain_low_low.free_current))
    dog_loop_writer.AddConstant(control_loop.Constant("kJ", "%f",
          drivetrain_low_low.J))
    dog_loop_writer.AddConstant(control_loop.Constant("kMass", "%f",
          drivetrain_low_low.m))
    dog_loop_writer.AddConstant(control_loop.Constant("kRobotRadius", "%f",
          drivetrain_low_low.rb))
    dog_loop_writer.AddConstant(control_loop.Constant("kWheelRadius", "%f",
          drivetrain_low_low.r))
    dog_loop_writer.AddConstant(control_loop.Constant("kR", "%f",
          drivetrain_low_low.resistance))
    dog_loop_writer.AddConstant(control_loop.Constant("kV", "%f",
          drivetrain_low_low.Kv))
    dog_loop_writer.AddConstant(control_loop.Constant("kT", "%f",
          drivetrain_low_low.Kt))
    dog_loop_writer.AddConstant(control_loop.Constant("kLowGearRatio", "%f",
          drivetrain_low_low.G_low))
    dog_loop_writer.AddConstant(control_loop.Constant("kHighGearRatio", "%f",
          drivetrain_high_high.G_high))

    dog_loop_writer.Write(argv[1], argv[2])

    kf_loop_writer = control_loop.ControlLoopWriter(
        "KFDrivetrain", [kf_drivetrain_low_low, kf_drivetrain_low_high,
                         kf_drivetrain_high_low, kf_drivetrain_high_high],
        namespaces = namespaces)
    kf_loop_writer.Write(argv[3], argv[4])