Add ability to set max velocity and acceleration
linear_system.py and angular_system.py are missing the ability to pass
in max_velocity and max_acceleration. Considering I use these a lot for
figuring out baseline profile parameters, I want them.
Change-Id: I9f49d103bed25e56bb13cb4d410a9fdd6435063b
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index e5dd199..8d375f8 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -164,7 +164,9 @@
duration=1.0,
use_profile=True,
kick_time=0.5,
- kick_magnitude=0.0):
+ kick_magnitude=0.0,
+ max_velocity=10.0,
+ max_acceleration=70.0):
"""Runs the plant with an initial condition and goal.
Args:
@@ -177,6 +179,8 @@
duration: float, time in seconds to run the simulation for.
kick_time: float, time in seconds to kick the robot.
kick_magnitude: float, disturbance in volts to apply.
+ max_velocity: float, The maximum velocity for the profile.
+ max_acceleration: float, The maximum acceleration for the profile.
"""
t_plot = []
x_plot = []
@@ -197,8 +201,8 @@
(plant.X, numpy.matrix(numpy.zeros((1, 1)))), axis=0)
profile = TrapezoidProfile(plant.dt)
- profile.set_maximum_acceleration(70.0)
- profile.set_maximum_velocity(10.0)
+ profile.set_maximum_acceleration(max_acceleration)
+ profile.set_maximum_velocity(max_velocity)
profile.SetGoal(goal[0, 0])
U_last = numpy.matrix(numpy.zeros((1, 1)))
@@ -326,11 +330,13 @@
kick_magnitude=2.0)
-def PlotMotion(params, R):
+def PlotMotion(params, R, max_velocity=10.0, max_acceleration=70.0):
"""Plots a trapezoidal motion.
Args:
R: numpy.matrix(2, 1), the goal,
+ max_velocity: float, The max velocity of the profile.
+ max_acceleration: float, The max acceleration of the profile.
"""
plant = AngularSystem(params, params.name)
controller = IntegralAngularSystem(params, params.name)
@@ -346,7 +352,9 @@
controller=controller,
observer=observer,
duration=2.0,
- use_profile=True)
+ use_profile=True,
+ max_velocity=max_velocity,
+ max_acceleration=max_acceleration)
def WriteAngularSystem(params, plant_files, controller_files, year_namespaces):
diff --git a/frc971/control_loops/python/linear_system.py b/frc971/control_loops/python/linear_system.py
index 7129e32..322e41a 100755
--- a/frc971/control_loops/python/linear_system.py
+++ b/frc971/control_loops/python/linear_system.py
@@ -171,7 +171,8 @@
use_profile=True,
kick_time=0.5,
kick_magnitude=0.0,
- max_velocity=0.3):
+ max_velocity=0.3,
+ max_acceleration=10.0):
"""Runs the plant with an initial condition and goal.
Args:
@@ -185,6 +186,7 @@
kick_time: float, time in seconds to kick the robot.
kick_magnitude: float, disturbance in volts to apply.
max_velocity: float, the max speed in m/s to profile.
+ max_acceleration: float, the max acceleration in m/s/s to profile.
"""
t_plot = []
x_plot = []
@@ -205,7 +207,7 @@
(plant.X, numpy.matrix(numpy.zeros((1, 1)))), axis=0)
profile = TrapezoidProfile(plant.dt)
- profile.set_maximum_acceleration(10.0)
+ profile.set_maximum_acceleration(max_acceleration)
profile.set_maximum_velocity(max_velocity)
profile.SetGoal(goal[0, 0])
@@ -334,12 +336,13 @@
kick_magnitude=2.0)
-def PlotMotion(params, R, max_velocity=0.3):
+def PlotMotion(params, R, max_velocity=0.3, max_acceleration=10.0):
"""Plots a trapezoidal motion.
Args:
R: numpy.matrix(2, 1), the goal,
max_velocity: float, The max velocity of the profile.
+ max_acceleration: float, The max acceleration of the profile.
"""
plant = LinearSystem(params, params.name)
controller = IntegralLinearSystem(params, params.name)
@@ -356,7 +359,8 @@
observer=observer,
duration=2.0,
use_profile=True,
- max_velocity=max_velocity)
+ max_velocity=max_velocity,
+ max_acceleration=max_acceleration)
def WriteLinearSystem(params, plant_files, controller_files, year_namespaces):