def WritePolyDrivetrainFullName(drivetrain_files, motor_files, namespaces,
directories, drivetrain_params):
vdrivetrain = VelocityDrivetrain(drivetrain_params)
dog_loop_writer = control_loop.ControlLoopWriter("VelocityDrivetrain", [
vdrivetrain.drivetrain_low_low, vdrivetrain.drivetrain_low_high,
vdrivetrain.drivetrain_high_low, vdrivetrain.drivetrain_high_high
],
namespaces=namespaces,
directories=directories)
dog_loop_writer.Write(drivetrain_files[0], drivetrain_files[1])
cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
cim_writer.Write(motor_files[0], motor_files[1])
def main(argv):
argv = FLAGS(argv)
vdrivetrain = VelocityDrivetrain()
if not FLAGS.plot:
if len(argv) != 5:
glog.fatal('Expected .h file name and .cc file name')
else:
namespaces = ['c2017', 'subsystems', 'drivetrain']
dog_loop_writer = control_loop.ControlLoopWriter(
"VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
vdrivetrain.drivetrain_low_high,
vdrivetrain.drivetrain_high_low,
vdrivetrain.drivetrain_high_high],
namespaces=namespaces)
dog_loop_writer.Write(argv[1], argv[2])
cim_writer = control_loop.ControlLoopWriter(
"CIM", [drivetrain.CIM()])
cim_writer.Write(argv[3], argv[4])
return
vl_plot = []
vr_plot = []
ul_plot = []
ur_plot = []
radius_plot = []
t_plot = []
left_gear_plot = []
right_gear_plot = []
vdrivetrain.left_shifter_position = 0.0
vdrivetrain.right_shifter_position = 0.0
vdrivetrain.left_gear = VelocityDrivetrain.LOW
vdrivetrain.right_gear = VelocityDrivetrain.LOW
glog.debug('K is %s', str(vdrivetrain.CurrentDrivetrain().K))
if vdrivetrain.left_gear is VelocityDrivetrain.HIGH:
glog.debug('Left is high')
else:
glog.debug('Left is low')
if vdrivetrain.right_gear is VelocityDrivetrain.HIGH:
glog.debug('Right is high')
else:
glog.debug('Right is low')
for t in numpy.arange(0, 1.7, vdrivetrain.dt):
if t < 0.5:
vdrivetrain.Update(throttle=0.00, steering=1.0)
elif t < 1.2:
vdrivetrain.Update(throttle=0.5, steering=1.0)
else:
vdrivetrain.Update(throttle=0.00, steering=1.0)
t_plot.append(t)
vl_plot.append(vdrivetrain.X[0, 0])
vr_plot.append(vdrivetrain.X[1, 0])
ul_plot.append(vdrivetrain.U[0, 0])
ur_plot.append(vdrivetrain.U[1, 0])
left_gear_plot.append((vdrivetrain.left_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.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
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])
def WriteDrivetrainFullName(drivetrain_files, kf_drivetrain_files, namespaces,
directories, drivetrain_params):
# Write the generated constants out to a file.
drivetrain_low_low = Drivetrain(name="DrivetrainLowLow",
left_low=True,
right_low=True,
drivetrain_params=drivetrain_params)
drivetrain_low_high = Drivetrain(name="DrivetrainLowHigh",
left_low=True,
right_low=False,
drivetrain_params=drivetrain_params)
drivetrain_high_low = Drivetrain(name="DrivetrainHighLow",
left_low=False,
right_low=True,
drivetrain_params=drivetrain_params)
drivetrain_high_high = Drivetrain(name="DrivetrainHighHigh",
left_low=False,
right_low=False,
drivetrain_params=drivetrain_params)
kf_drivetrain_low_low = KFDrivetrain(name="KFDrivetrainLowLow",
left_low=True,
right_low=True,
drivetrain_params=drivetrain_params)
kf_drivetrain_low_high = KFDrivetrain(name="KFDrivetrainLowHigh",
left_low=True,
right_low=False,
drivetrain_params=drivetrain_params)
kf_drivetrain_high_low = KFDrivetrain(name="KFDrivetrainHighLow",
left_low=False,
right_low=True,
drivetrain_params=drivetrain_params)
kf_drivetrain_high_high = KFDrivetrain(name="KFDrivetrainHighHigh",
left_low=False,
right_low=False,
drivetrain_params=drivetrain_params)
dog_loop_writer = control_loop.ControlLoopWriter("Drivetrain", [
drivetrain_low_low, drivetrain_low_high, drivetrain_high_low,
drivetrain_high_high
],
namespaces=namespaces,
directories=directories)
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("kFreeSpeed", "%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.mass))
dog_loop_writer.AddConstant(
control_loop.Constant("kRobotRadius", "%f",
drivetrain_low_low.robot_radius))
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.AddConstant(
control_loop.Constant(
"kHighOutputRatio", "%f",
drivetrain_high_high.G_high * drivetrain_high_high.r))
dog_loop_writer.AddConstant(
control_loop.Constant("kHighAlpha", "%f",
drivetrain_high_high.A_continuous[1, 1]))
dog_loop_writer.AddConstant(
control_loop.Constant("kHighGamma", "%f",
drivetrain_high_high.A_continuous[1, 3]))
dog_loop_writer.AddConstant(
control_loop.Constant("kHighBeta", "%f",
drivetrain_high_high.B_continuous[1, 0]))
dog_loop_writer.AddConstant(
control_loop.Constant("kHighDelta", "%f",
drivetrain_high_high.B_continuous[1, 1]))
dog_loop_writer.AddConstant(
control_loop.Constant("kLowAlpha", "%f",
drivetrain_low_low.A_continuous[1, 1]))
dog_loop_writer.AddConstant(
control_loop.Constant("kLowGamma", "%f",
drivetrain_low_low.A_continuous[1, 3]))
dog_loop_writer.AddConstant(
control_loop.Constant("kLowBeta", "%f",
drivetrain_low_low.B_continuous[1, 0]))
dog_loop_writer.AddConstant(
control_loop.Constant("kLowDelta", "%f",
drivetrain_low_low.B_continuous[1, 1]))
dog_loop_writer.Write(drivetrain_files[0], drivetrain_files[1])
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,
directories=directories)
kf_loop_writer.Write(kf_drivetrain_files[0], kf_drivetrain_files[1])