def main(argv):
scenario_plotter = ScenarioPlotter()
elbow = Elbow()
elbow_controller = IntegralElbow()
observer_elbow = IntegralElbow()
# Test moving the elbow with constant separation.
initial_X = numpy.matrix([[0.0], [0.0]])
initial_X = numpy.matrix([[0.707], [0.707]])
R = numpy.matrix([[1.0], [3.0], [1.0]])
scenario_plotter.run_test(elbow, goal=R, controller_elbow=elbow_controller,
observer_elbow=observer_elbow, iterations=2000)
scenario_plotter.Plot(elbow)
# Write the generated constants out to a file.
print(len(argv))
if len(argv) != 5:
glog.fatal('Expected .h file name and .cc file name for the elbow and integral elbow.')
else:
namespaces = ['shit']
elbow = Elbow('Elbow')
loop_writer = control_loop.ControlLoopWriter(
'Elbow', [elbow], namespaces=namespaces)
loop_writer.Write(argv[1], argv[2])
integral_elbow = IntegralElbow('IntegralElbow')
integral_loop_writer = control_loop.ControlLoopWriter(
'IntegralElbow', [integral_elbow], namespaces=namespaces)
integral_loop_writer.Write(argv[3], argv[4])
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])
def main(argv):
vdrivetrain = VelocityDrivetrain()
if len(argv) != 7:
print "Expected .h file name and .cc file name"
else:
dog_loop_writer = control_loop.ControlLoopWriter(
"VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
vdrivetrain.drivetrain_low_high,
vdrivetrain.drivetrain_high_low,
vdrivetrain.drivetrain_high_high],
namespaces=['drivetrain', 'control_loops'])
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
else:
dog_loop_writer.Write(argv[1], argv[2])
cim_writer = control_loop.ControlLoopWriter(
"CIM", [drivetrain.CIM()],
namespaces=['drivetrain', 'control_loops'])
if argv[5][-3:] == '.cc':
cim_writer.Write(argv[6], argv[5])
else:
cim_writer.Write(argv[5], argv[6])
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
print "K is", vdrivetrain.CurrentDrivetrain().K
if vdrivetrain.left_gear is VelocityDrivetrain.HIGH:
print "Left is high"
else:
print "Left is low"
if vdrivetrain.right_gear is VelocityDrivetrain.HIGH:
print "Right is high"
else:
print "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)
cim_velocity_plot = []
cim_voltage_plot = []
cim_time = []
cim = drivetrain.CIM()
R = numpy.matrix([[300]])
for t in numpy.arange(0, 0.5, cim.dt):
U = numpy.clip(cim.K * (R - cim.X) + R / cim.Kv, cim.U_min, cim.U_max)
cim.Update(U)
cim_velocity_plot.append(cim.X[0, 0])
cim_voltage_plot.append(U[0, 0] * 10)
cim_time.append(t)
pylab.plot(cim_time, cim_velocity_plot, label='cim spinup')
pylab.plot(cim_time, cim_voltage_plot, label='cim voltage')
pylab.legend()
pylab.show()
# 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