Switched to 5ms cycles on the control loops.
Change-Id: I1aae3b30a9c422f1920ccb1c15e035ae847f85a9
diff --git a/y2014/actors/BUILD b/y2014/actors/BUILD
index 086d8b4..6fea9eb 100644
--- a/y2014/actors/BUILD
+++ b/y2014/actors/BUILD
@@ -82,6 +82,7 @@
'//third_party/eigen',
'//aos/common/util:trapezoid_profile',
'//y2014/control_loops/drivetrain:drivetrain_queue',
+ '//frc971/control_loops:state_feedback_loop',
],
)
diff --git a/y2014/actors/drivetrain_actor.cc b/y2014/actors/drivetrain_actor.cc
index 1f1a678..aa1f6f2 100644
--- a/y2014/actors/drivetrain_actor.cc
+++ b/y2014/actors/drivetrain_actor.cc
@@ -19,10 +19,13 @@
namespace actors {
DrivetrainActor::DrivetrainActor(actors::DrivetrainActionQueueGroup* s)
- : aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s) {}
+ : aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s),
+ loop_(constants::GetValues().make_drivetrain_loop()) {
+ loop_.set_controller_index(3);
+}
bool DrivetrainActor::RunAction(const actors::DrivetrainActionParams ¶ms) {
- static const auto K = constants::GetValues().make_drivetrain_loop().K();
+ static const auto K = loop_.K();
const double yoffset = params.y_offset;
const double turn_offset =
@@ -30,8 +33,8 @@
LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
// Measured conversion to get the distance right.
- ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(10));
- ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(10));
+ ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(5));
+ ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(5));
const double goal_velocity = 0.0;
const double epsilon = 0.01;
::Eigen::Matrix<double, 2, 1> left_goal_state, right_goal_state;
@@ -45,7 +48,7 @@
constants::GetValues().turn_width / 2.0);
while (true) {
- ::aos::time::PhasedLoopXMS(10, 5000);
+ ::aos::time::PhasedLoopXMS(5, 2500);
control_loops::drivetrain_queue.status.FetchLatest();
if (control_loops::drivetrain_queue.status.get()) {
diff --git a/y2014/actors/drivetrain_actor.h b/y2014/actors/drivetrain_actor.h
index 583121b..888b2c0 100644
--- a/y2014/actors/drivetrain_actor.h
+++ b/y2014/actors/drivetrain_actor.h
@@ -5,6 +5,7 @@
#include "aos/common/actions/actor.h"
#include "aos/common/actions/actions.h"
+#include "frc971/control_loops/state_feedback_loop.h"
#include "y2014/actors/drivetrain_action.q.h"
@@ -17,6 +18,9 @@
explicit DrivetrainActor(DrivetrainActionQueueGroup* s);
bool RunAction(const actors::DrivetrainActionParams ¶ms) override;
+
+ private:
+ StateFeedbackLoop<4, 2, 2> loop_;
};
typedef ::aos::common::actions::TypedAction<DrivetrainActionQueueGroup>
diff --git a/y2014/control_loops/python/arm.py b/y2014/control_loops/python/arm.py
deleted file mode 100755
index e17990a..0000000
--- a/y2014/control_loops/python/arm.py
+++ /dev/null
@@ -1,409 +0,0 @@
-#!/usr/bin/python
-
-import control_loop
-import controls
-import polytope
-import polydrivetrain
-import numpy
-import math
-import sys
-import matplotlib
-from matplotlib import pylab
-
-
-class Arm(control_loop.ControlLoop):
- def __init__(self, name="Arm", mass=None):
- super(Arm, self).__init__(name)
- # Stall Torque in N m
- self.stall_torque = 0.476
- # Stall Current in Amps
- self.stall_current = 80.730
- # Free Speed in RPM
- self.free_speed = 13906.0
- # Free Current in Amps
- self.free_current = 5.820
- # Mass of the arm
- if mass is None:
- self.mass = 13.0
- else:
- self.mass = mass
-
- # Resistance of the motor
- self.R = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
- (12.0 - self.R * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Gear ratio
- self.G = (44.0 / 12.0) * (54.0 / 14.0) * (54.0 / 14.0) * (44.0 / 20.0) * (72.0 / 16.0)
- # Fridge arm length
- self.r = 32 * 0.0254
- # Control loop time step
- self.dt = 0.005
-
- # Arm moment of inertia
- self.J = self.r * self.mass
-
- # Arm left/right spring constant (N*m / radian)
- self.spring = 100.0
-
- # State is [average position, average velocity,
- # position difference/2, velocity difference/2]
- # Position difference is 1 - 2
- # Input is [Voltage 1, Voltage 2]
-
- self.C1 = self.spring / (self.J * 0.5)
- self.C2 = self.Kt * self.G / (self.J * 0.5 * self.R)
- self.C3 = self.G * self.G * self.Kt / (self.R * self.J * 0.5 * self.Kv)
-
- self.A_continuous = numpy.matrix(
- [[0, 1, 0, 0],
- [0, -self.C3, 0, 0],
- [0, 0, 0, 1],
- [0, 0, -self.C1 * 2.0, -self.C3]])
-
- print 'Full speed is', self.C2 / self.C3 * 12.0
-
- print 'Stall arm difference is', 12.0 * self.C2 / self.C1
- print 'Stall arm difference first principles is', self.stall_torque * self.G / self.spring
-
- print '5 degrees of arm error is', self.spring / self.r * (math.pi * 5.0 / 180.0)
-
- # Start with the unmodified input
- self.B_continuous = numpy.matrix(
- [[0, 0],
- [self.C2 / 2.0, self.C2 / 2.0],
- [0, 0],
- [self.C2 / 2.0, -self.C2 / 2.0]])
-
- self.C = numpy.matrix([[1, 0, 1, 0],
- [1, 0, -1, 0]])
- self.D = numpy.matrix([[0, 0],
- [0, 0]])
-
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
-
- controlability = controls.ctrb(self.A, self.B);
- print 'Rank of augmented controlability matrix.', numpy.linalg.matrix_rank(
- controlability)
-
- q_pos = 0.02
- q_vel = 0.300
- q_pos_diff = 0.005
- q_vel_diff = 0.13
- self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
- [0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
- [0.0, 0.0, (1.0 / (q_pos_diff ** 2.0)), 0.0],
- [0.0, 0.0, 0.0, (1.0 / (q_vel_diff ** 2.0))]])
-
- self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
- [0.0, 1.0 / (12.0 ** 2.0)]])
- self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- print 'Controller'
- print self.K
-
- print 'Controller Poles'
- print numpy.linalg.eig(self.A - self.B * self.K)[0]
-
- self.rpl = 0.20
- self.ipl = 0.05
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl,
- self.rpl - 1j * self.ipl])
-
- # The box formed by U_min and U_max must encompass all possible values,
- # or else Austin's code gets angry.
- self.U_max = numpy.matrix([[12.0], [12.0]])
- self.U_min = numpy.matrix([[-12.0], [-12.0]])
-
- print 'Observer (Converted to a KF)', numpy.linalg.inv(self.A) * self.L
-
- self.InitializeState()
-
-
-class IntegralArm(Arm):
- def __init__(self, name="IntegralArm", mass=None):
- super(IntegralArm, self).__init__(name=name, mass=mass)
-
- self.A_continuous_unaugmented = self.A_continuous
- self.A_continuous = numpy.matrix(numpy.zeros((5, 5)))
- self.A_continuous[0:4, 0:4] = self.A_continuous_unaugmented
- self.A_continuous[1, 4] = self.C2
-
- # Start with the unmodified input
- self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((5, 2)))
- self.B_continuous[0:4, 0:2] = self.B_continuous_unaugmented
-
- self.C_unaugmented = self.C
- self.C = numpy.matrix(numpy.zeros((2, 5)))
- self.C[0:2, 0:4] = self.C_unaugmented
-
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
- print 'A cont', self.A_continuous
- print 'B cont', self.B_continuous
- print 'A discrete', self.A
-
- q_pos = 0.08
- q_vel = 0.40
-
- q_pos_diff = 0.08
- q_vel_diff = 0.40
- q_voltage = 6.0
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0, 0.0],
- [0.0, (q_vel ** 2.0), 0.0, 0.0, 0.0],
- [0.0, 0.0, (q_pos_diff ** 2.0), 0.0, 0.0],
- [0.0, 0.0, 0.0, (q_vel_diff ** 2.0), 0.0],
- [0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0)]])
-
- r_volts = 0.05
- self.R = numpy.matrix([[(r_volts ** 2.0), 0.0],
- [0.0, (r_volts ** 2.0)]])
-
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
-
- self.U_max = numpy.matrix([[12.0], [12.0]])
- self.U_min = numpy.matrix([[-12.0], [-12.0]])
-
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((2, 5)));
- self.K[0:2, 0:4] = self.K_unaugmented
- self.K[0, 4] = 1;
- self.K[1, 4] = 1;
- print 'Kal', self.KalmanGain
- self.L = self.A * self.KalmanGain
-
- self.InitializeState()
-
-
-def CapU(U):
- if U[0, 0] - U[1, 0] > 24:
- return numpy.matrix([[12], [-12]])
- elif U[0, 0] - U[1, 0] < -24:
- return numpy.matrix([[-12], [12]])
- else:
- max_u = max(U[0, 0], U[1, 0])
- min_u = min(U[0, 0], U[1, 0])
- if max_u > 12:
- return U - (max_u - 12)
- if min_u < -12:
- return U - (min_u + 12)
- return U
-
-
-def run_test(arm, initial_X, goal, max_separation_error=0.01,
- show_graph=True, iterations=200, controller_arm=None,
- observer_arm=None):
- """Runs the arm plant with an initial condition and goal.
-
- The tests themselves are not terribly sophisticated; I just test for
- whether the goal has been reached and whether the separation goes
- outside of the initial and goal values by more than max_separation_error.
- Prints out something for a failure of either condition and returns
- False if tests fail.
- Args:
- arm: arm object to use.
- initial_X: starting state.
- goal: goal state.
- show_graph: Whether or not to display a graph showing the changing
- states and voltages.
- iterations: Number of timesteps to run the model for.
- controller_arm: arm object to get K from, or None if we should
- use arm.
- observer_arm: arm object to use for the observer, or None if we should
- use the actual state.
- """
-
- arm.X = initial_X
-
- if controller_arm is None:
- controller_arm = arm
-
- if observer_arm is not None:
- observer_arm.X_hat = initial_X + 0.01
- observer_arm.X_hat = initial_X
-
- # Various lists for graphing things.
- t = []
- x_avg = []
- x_sep = []
- x_hat_avg = []
- x_hat_sep = []
- v_avg = []
- v_sep = []
- u_left = []
- u_right = []
-
- sep_plot_gain = 100.0
-
- for i in xrange(iterations):
- X_hat = arm.X
- if observer_arm is not None:
- X_hat = observer_arm.X_hat
- x_hat_avg.append(observer_arm.X_hat[0, 0])
- x_hat_sep.append(observer_arm.X_hat[2, 0] * sep_plot_gain)
- U = controller_arm.K * (goal - X_hat)
- U = CapU(U)
- x_avg.append(arm.X[0, 0])
- v_avg.append(arm.X[1, 0])
- x_sep.append(arm.X[2, 0] * sep_plot_gain)
- v_sep.append(arm.X[3, 0])
- if observer_arm is not None:
- observer_arm.PredictObserver(U)
- arm.Update(U)
- if observer_arm is not None:
- observer_arm.Y = arm.Y
- observer_arm.CorrectObserver(U)
-
- t.append(i * arm.dt)
- u_left.append(U[0, 0])
- u_right.append(U[1, 0])
-
- print numpy.linalg.inv(arm.A)
- print "delta time is ", arm.dt
- print "Velocity at t=0 is ", x_avg[0], v_avg[0], x_sep[0], v_sep[0]
- print "Velocity at t=1+dt is ", x_avg[1], v_avg[1], x_sep[1], v_sep[1]
-
- if show_graph:
- pylab.subplot(2, 1, 1)
- pylab.plot(t, x_avg, label='x avg')
- pylab.plot(t, x_sep, label='x sep')
- if observer_arm is not None:
- pylab.plot(t, x_hat_avg, label='x_hat avg')
- pylab.plot(t, x_hat_sep, label='x_hat sep')
- pylab.legend()
-
- pylab.subplot(2, 1, 2)
- pylab.plot(t, u_left, label='u left')
- pylab.plot(t, u_right, label='u right')
- pylab.legend()
- pylab.show()
-
-
-def run_integral_test(arm, initial_X, goal, observer_arm, disturbance,
- max_separation_error=0.01, show_graph=True,
- iterations=400):
- """Runs the integral control arm plant with an initial condition and goal.
-
- The tests themselves are not terribly sophisticated; I just test for
- whether the goal has been reached and whether the separation goes
- outside of the initial and goal values by more than max_separation_error.
- Prints out something for a failure of either condition and returns
- False if tests fail.
- Args:
- arm: arm object to use.
- initial_X: starting state.
- goal: goal state.
- observer_arm: arm object to use for the observer.
- show_graph: Whether or not to display a graph showing the changing
- states and voltages.
- iterations: Number of timesteps to run the model for.
- disturbance: Voltage missmatch between controller and model.
- """
-
- arm.X = initial_X
-
- # Various lists for graphing things.
- t = []
- x_avg = []
- x_sep = []
- x_hat_avg = []
- x_hat_sep = []
- v_avg = []
- v_sep = []
- u_left = []
- u_right = []
- u_error = []
-
- sep_plot_gain = 100.0
-
- unaugmented_goal = goal
- goal = numpy.matrix(numpy.zeros((5, 1)))
- goal[0:4, 0] = unaugmented_goal
-
- for i in xrange(iterations):
- X_hat = observer_arm.X_hat[0:4]
-
- x_hat_avg.append(observer_arm.X_hat[0, 0])
- x_hat_sep.append(observer_arm.X_hat[2, 0] * sep_plot_gain)
-
- U = observer_arm.K * (goal - observer_arm.X_hat)
- u_error.append(observer_arm.X_hat[4,0])
- U = CapU(U)
- x_avg.append(arm.X[0, 0])
- v_avg.append(arm.X[1, 0])
- x_sep.append(arm.X[2, 0] * sep_plot_gain)
- v_sep.append(arm.X[3, 0])
-
- observer_arm.PredictObserver(U)
-
- arm.Update(U + disturbance)
- observer_arm.Y = arm.Y
- observer_arm.CorrectObserver(U)
-
- t.append(i * arm.dt)
- u_left.append(U[0, 0])
- u_right.append(U[1, 0])
-
- print 'End is', observer_arm.X_hat[4, 0]
-
- if show_graph:
- pylab.subplot(2, 1, 1)
- pylab.plot(t, x_avg, label='x avg')
- pylab.plot(t, x_sep, label='x sep')
- if observer_arm is not None:
- pylab.plot(t, x_hat_avg, label='x_hat avg')
- pylab.plot(t, x_hat_sep, label='x_hat sep')
- pylab.legend()
-
- pylab.subplot(2, 1, 2)
- pylab.plot(t, u_left, label='u left')
- pylab.plot(t, u_right, label='u right')
- pylab.plot(t, u_error, label='u error')
- pylab.legend()
- pylab.show()
-
-
-def main(argv):
- loaded_mass = 25
- #loaded_mass = 0
- arm = Arm(mass=13 + loaded_mass)
- #arm_controller = Arm(mass=13 + 15)
- #observer_arm = Arm(mass=13 + 15)
- #observer_arm = None
-
- integral_arm = IntegralArm(mass=13 + loaded_mass)
- integral_arm.X_hat[0, 0] += 0.02
- integral_arm.X_hat[2, 0] += 0.02
- integral_arm.X_hat[4] = 0
-
- # Test moving the arm with constant separation.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- run_integral_test(arm, initial_X, R, integral_arm, disturbance=2)
-
- # Write the generated constants out to a file.
- if len(argv) != 5:
- print "Expected .h file name and .cc file name for the arm and augmented arm."
- else:
- arm = Arm("Arm", mass=13)
- loop_writer = control_loop.ControlLoopWriter("Arm", [arm])
- if argv[1][-3:] == '.cc':
- loop_writer.Write(argv[2], argv[1])
- else:
- loop_writer.Write(argv[1], argv[2])
-
- integral_arm = IntegralArm("IntegralArm", mass=13)
- loop_writer = control_loop.ControlLoopWriter("IntegralArm", [integral_arm])
- if argv[3][-3:] == '.cc':
- loop_writer.Write(argv[4], argv[3])
- else:
- loop_writer.Write(argv[3], argv[4])
-
-if __name__ == '__main__':
- sys.exit(main(sys.argv))
diff --git a/y2014/control_loops/python/claw.py b/y2014/control_loops/python/claw.py
index 6808ce6..91f48de 100755
--- a/y2014/control_loops/python/claw.py
+++ b/y2014/control_loops/python/claw.py
@@ -33,7 +33,7 @@
# Gear ratio
self.G = 14.0 / 48.0 * 18.0 / 32.0 * 18.0 / 66.0 * 12.0 / 60.0
# Control loop time step
- self.dt = 0.01
+ self.dt = 0.005
# State is [bottom position, bottom velocity, top - bottom position,
# top - bottom velocity]
@@ -95,8 +95,10 @@
self.A_diff, self.B_diff = controls.c2d(
self.A_diff_cont, self.B_diff_cont, self.dt)
- self.K_bottom = controls.dplace(self.A_bottom, self.B_bottom, [.75 + 0.1j, .75 - 0.1j])
- self.K_diff = controls.dplace(self.A_diff, self.B_diff, [.75 + 0.1j, .75 - 0.1j])
+ self.K_bottom = controls.dplace(self.A_bottom, self.B_bottom,
+ [0.87 + 0.05j, 0.87 - 0.05j])
+ self.K_diff = controls.dplace(self.A_diff, self.B_diff,
+ [0.85 + 0.05j, 0.85 - 0.05j])
print "K_diff", self.K_diff
print "K_bottom", self.K_bottom
@@ -144,8 +146,8 @@
print "eigenvalues"
print numpy.linalg.eig(F)[0]
- self.rpl = .05
- self.ipl = 0.010
+ self.rpl = .09
+ self.ipl = 0.030
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl,
diff --git a/y2014/control_loops/python/drivetrain.py b/y2014/control_loops/python/drivetrain.py
index b94a501..32016b1 100755
--- a/y2014/control_loops/python/drivetrain.py
+++ b/y2014/control_loops/python/drivetrain.py
@@ -28,7 +28,7 @@
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Control loop time step
- self.dt = 0.010
+ self.dt = 0.005
# State feedback matrices
self.A_continuous = numpy.matrix(
@@ -90,7 +90,7 @@
self.Gr = self.G_high
# Control loop time step
- self.dt = 0.010
+ self.dt = 0.005
# These describe the way that a given side of a robot will be influenced
# by the other side. Units of 1 / kg.
@@ -128,7 +128,7 @@
self.lp = 0.83
self.PlaceControllerPoles([self.hp, self.lp, self.hp, self.lp])
#print self.K
- q_pos = 0.07
+ q_pos = 0.12
q_vel = 1.0
self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
[0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
@@ -140,8 +140,9 @@
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
#print self.A
#print self.B
- #print self.K
- #print numpy.linalg.eig(self.A - self.B * self.K)[0]
+ print "DT K", name
+ print self.K
+ print numpy.linalg.eig(self.A - self.B * self.K)[0]
self.hlp = 0.3
self.llp = 0.4
diff --git a/y2014/control_loops/python/elevator.py b/y2014/control_loops/python/elevator.py
deleted file mode 100755
index fba72c8..0000000
--- a/y2014/control_loops/python/elevator.py
+++ /dev/null
@@ -1,246 +0,0 @@
-#!/usr/bin/python
-
-import control_loop
-import controls
-import polytope
-import polydrivetrain
-import numpy
-import sys
-import matplotlib
-from matplotlib import pylab
-
-class Elevator(control_loop.ControlLoop):
- def __init__(self, name="Elevator", mass=None):
- super(Elevator, self).__init__(name)
- # Stall Torque in N m
- self.stall_torque = 0.476
- # Stall Current in Amps
- self.stall_current = 80.730
- # Free Speed in RPM
- self.free_speed = 13906.0
- # Free Current in Amps
- self.free_current = 5.820
- # Mass of the elevator
- if mass is None:
- self.mass = 13.0
- else:
- self.mass = mass
-
- # Resistance of the motor
- self.R = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
- (12.0 - self.R * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Gear ratio
- self.G = (56.0 / 12.0) * (84.0 / 14.0)
- # Pulley diameter
- self.r = 32 * 0.005 / numpy.pi / 2.0
- # Control loop time step
- self.dt = 0.005
-
- # Elevator left/right spring constant (N/m)
- self.spring = 800.0
-
- # State is [average position, average velocity,
- # position difference/2, velocity difference/2]
- # Input is [V_left, V_right]
-
- C1 = self.spring / (self.mass * 0.5)
- C2 = self.Kt * self.G / (self.mass * 0.5 * self.r * self.R)
- C3 = self.G * self.G * self.Kt / (
- self.R * self.r * self.r * self.mass * 0.5 * self.Kv)
-
- self.A_continuous = numpy.matrix(
- [[0, 1, 0, 0],
- [0, -C3, 0, 0],
- [0, 0, 0, 1],
- [0, 0, -C1 * 2.0, -C3]])
-
- print "Full speed is", C2 / C3 * 12.0
-
- # Start with the unmodified input
- self.B_continuous = numpy.matrix(
- [[0, 0],
- [C2 / 2.0, C2 / 2.0],
- [0, 0],
- [C2 / 2.0, -C2 / 2.0]])
-
- self.C = numpy.matrix([[1, 0, 1, 0],
- [1, 0, -1, 0]])
- self.D = numpy.matrix([[0, 0],
- [0, 0]])
-
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
-
- print self.A
-
- controlability = controls.ctrb(self.A, self.B);
- print "Rank of augmented controlability matrix.", numpy.linalg.matrix_rank(
- controlability)
-
- q_pos = 0.02
- q_vel = 0.400
- q_pos_diff = 0.01
- q_vel_diff = 0.45
- self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
- [0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
- [0.0, 0.0, (1.0 / (q_pos_diff ** 2.0)), 0.0],
- [0.0, 0.0, 0.0, (1.0 / (q_vel_diff ** 2.0))]])
-
- self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
- [0.0, 1.0 / (12.0 ** 2.0)]])
- self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- print self.K
-
- print numpy.linalg.eig(self.A - self.B * self.K)[0]
-
- self.rpl = 0.20
- self.ipl = 0.05
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl,
- self.rpl - 1j * self.ipl])
-
- # The box formed by U_min and U_max must encompass all possible values,
- # or else Austin's code gets angry.
- self.U_max = numpy.matrix([[12.0], [12.0]])
- self.U_min = numpy.matrix([[-12.0], [-12.0]])
-
- self.InitializeState()
-
-
-def CapU(U):
- if U[0, 0] - U[1, 0] > 24:
- return numpy.matrix([[12], [-12]])
- elif U[0, 0] - U[1, 0] < -24:
- return numpy.matrix([[-12], [12]])
- else:
- max_u = max(U[0, 0], U[1, 0])
- min_u = min(U[0, 0], U[1, 0])
- if max_u > 12:
- return U - (max_u - 12)
- if min_u < -12:
- return U - (min_u + 12)
- return U
-
-
-def run_test(elevator, initial_X, goal, max_separation_error=0.01,
- show_graph=True, iterations=200, controller_elevator=None,
- observer_elevator=None):
- """Runs the elevator plant with an initial condition and goal.
-
- The tests themselves are not terribly sophisticated; I just test for
- whether the goal has been reached and whether the separation goes
- outside of the initial and goal values by more than max_separation_error.
- Prints out something for a failure of either condition and returns
- False if tests fail.
- Args:
- elevator: elevator object to use.
- initial_X: starting state.
- goal: goal state.
- show_graph: Whether or not to display a graph showing the changing
- states and voltages.
- iterations: Number of timesteps to run the model for.
- controller_elevator: elevator object to get K from, or None if we should
- use elevator.
- observer_elevator: elevator object to use for the observer, or None if we
- should use the actual state.
- """
-
- elevator.X = initial_X
-
- if controller_elevator is None:
- controller_elevator = elevator
-
- if observer_elevator is not None:
- observer_elevator.X_hat = initial_X + 0.01
- observer_elevator.X_hat = initial_X
-
- # Various lists for graphing things.
- t = []
- x_avg = []
- x_sep = []
- x_hat_avg = []
- x_hat_sep = []
- v_avg = []
- v_sep = []
- u_left = []
- u_right = []
-
- sep_plot_gain = 100.0
-
- for i in xrange(iterations):
- X_hat = elevator.X
- if observer_elevator is not None:
- X_hat = observer_elevator.X_hat
- x_hat_avg.append(observer_elevator.X_hat[0, 0])
- x_hat_sep.append(observer_elevator.X_hat[2, 0] * sep_plot_gain)
- U = controller_elevator.K * (goal - X_hat)
- U = CapU(U)
- x_avg.append(elevator.X[0, 0])
- v_avg.append(elevator.X[1, 0])
- x_sep.append(elevator.X[2, 0] * sep_plot_gain)
- v_sep.append(elevator.X[3, 0])
- if observer_elevator is not None:
- observer_elevator.PredictObserver(U)
- elevator.Update(U)
- if observer_elevator is not None:
- observer_elevator.Y = elevator.Y
- observer_elevator.CorrectObserver(U)
-
- t.append(i * elevator.dt)
- u_left.append(U[0, 0])
- u_right.append(U[1, 0])
-
- print numpy.linalg.inv(elevator.A)
- print "delta time is ", elevator.dt
- print "Velocity at t=0 is ", x_avg[0], v_avg[0], x_sep[0], v_sep[0]
- print "Velocity at t=1+dt is ", x_avg[1], v_avg[1], x_sep[1], v_sep[1]
-
- if show_graph:
- pylab.subplot(2, 1, 1)
- pylab.plot(t, x_avg, label='x avg')
- pylab.plot(t, x_sep, label='x sep')
- if observer_elevator is not None:
- pylab.plot(t, x_hat_avg, label='x_hat avg')
- pylab.plot(t, x_hat_sep, label='x_hat sep')
- pylab.legend()
-
- pylab.subplot(2, 1, 2)
- pylab.plot(t, u_left, label='u left')
- pylab.plot(t, u_right, label='u right')
- pylab.legend()
- pylab.show()
-
-
-def main(argv):
- loaded_mass = 25
- #loaded_mass = 0
- elevator = Elevator(mass=13 + loaded_mass)
- elevator_controller = Elevator(mass=13 + 15)
- observer_elevator = Elevator(mass=13 + 15)
- #observer_elevator = None
-
- # Test moving the elevator with constant separation.
- initial_X = numpy.matrix([[0.0], [0.0], [0.01], [0.0]])
- #initial_X = numpy.matrix([[0.0], [0.0], [0.00], [0.0]])
- R = numpy.matrix([[1.0], [0.0], [0.0], [0.0]])
- run_test(elevator, initial_X, R, controller_elevator=elevator_controller,
- observer_elevator=observer_elevator)
-
- # Write the generated constants out to a file.
- if len(argv) != 3:
- print "Expected .h file name and .cc file name for the elevator."
- else:
- elevator = Elevator("Elevator")
- loop_writer = control_loop.ControlLoopWriter("Elevator", [elevator])
- if argv[1][-3:] == '.cc':
- loop_writer.Write(argv[2], argv[1])
- else:
- loop_writer.Write(argv[1], argv[2])
-
-if __name__ == '__main__':
- sys.exit(main(sys.argv))
diff --git a/y2014/control_loops/python/polydrivetrain.py b/y2014/control_loops/python/polydrivetrain.py
index 3dafd21..fcf4b5b 100755
--- a/y2014/control_loops/python/polydrivetrain.py
+++ b/y2014/control_loops/python/polydrivetrain.py
@@ -102,7 +102,7 @@
super(VelocityDrivetrainModel, self).__init__(name)
self._drivetrain = drivetrain.Drivetrain(left_low=left_low,
right_low=right_low)
- self.dt = 0.01
+ self.dt = 0.005
self.A_continuous = numpy.matrix(
[[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
[self._drivetrain.A_continuous[3, 1], self._drivetrain.A_continuous[3, 3]]])
@@ -119,7 +119,7 @@
# FF * X = U (steady state)
self.FF = self.B.I * (numpy.eye(2) - self.A)
- self.PlaceControllerPoles([0.6, 0.6])
+ self.PlaceControllerPoles([0.8, 0.8])
self.PlaceObserverPoles([0.02, 0.02])
self.G_high = self._drivetrain.G_high
@@ -168,7 +168,7 @@
[[-12.0000000000],
[-12.0000000000]])
- self.dt = 0.01
+ self.dt = 0.005
self.R = numpy.matrix(
[[0.0],
diff --git a/y2014/control_loops/python/shooter.py b/y2014/control_loops/python/shooter.py
index 7324b18..379bd0a 100755
--- a/y2014/control_loops/python/shooter.py
+++ b/y2014/control_loops/python/shooter.py
@@ -38,7 +38,7 @@
self.G = 10.0 / 40.0 * 20.0 / 54.0 * 24.0 / 54.0 * 20.0 / 84.0 * 16.0 * (3.0 / 8.0) / (2.0 * numpy.pi) * 0.0254
# Control loop time step
- self.dt = 0.01
+ self.dt = 0.005
# State feedback matrices
self.A_continuous = numpy.matrix(
diff --git a/y2014/control_loops/shooter/shooter.cc b/y2014/control_loops/shooter/shooter.cc
index 29682f2..621c2d4 100644
--- a/y2014/control_loops/shooter/shooter.cc
+++ b/y2014/control_loops/shooter/shooter.cc
@@ -42,7 +42,7 @@
voltage_ = std::max(-max_voltage_, voltage_);
mutable_U(0, 0) = voltage_ - old_voltage;
- LOG_STRUCT(DEBUG, "output", ShooterVoltageToLog(X_hat(2, 0), voltage_));
+ LOG_STRUCT(DEBUG, "shooter_output", ShooterVoltageToLog(X_hat(2, 0), voltage_));
last_voltage_ = voltage_;
capped_goal_ = false;
@@ -402,8 +402,11 @@
}
if (load_timeout_ < Time::Now()) {
if (position) {
- if (!position->pusher_distal.current ||
- !position->pusher_proximal.current) {
+ // If none of the sensors is triggered, estop.
+ // Otherwise, trigger anyways if it has been 0.5 seconds more.
+ if (!(position->pusher_distal.current ||
+ position->pusher_proximal.current) ||
+ (load_timeout_ + Time::InSeconds(0.5) < Time::Now()) {
state_ = STATE_ESTOP;
LOG(ERROR, "Estopping because took too long to load.\n");
}
@@ -534,7 +537,7 @@
// Also move on if it times out.
if (((::std::abs(firing_starting_position_ -
shooter_.absolute_position()) > 0.0005 &&
- cycles_not_moved_ > 3) ||
+ cycles_not_moved_ > 6) ||
Time::Now() > shot_end_time_) &&
::aos::robot_state->voltage_battery > 10.5) {
state_ = STATE_REQUEST_LOAD;
@@ -679,6 +682,10 @@
last_proximal_current_ = position->pusher_proximal.current;
}
+ status->absolute_position = shooter_.absolute_position();
+ status->absolute_velocity = shooter_.absolute_velocity();
+ status->state = state_;
+
status->shots = shot_count_;
}
diff --git a/y2014/control_loops/shooter/shooter.h b/y2014/control_loops/shooter/shooter.h
index 0efbbd0..93b45e0 100644
--- a/y2014/control_loops/shooter/shooter.h
+++ b/y2014/control_loops/shooter/shooter.h
@@ -114,7 +114,7 @@
};
const Time kUnloadTimeout = Time::InSeconds(10);
-const Time kLoadTimeout = Time::InSeconds(2);
+const Time kLoadTimeout = Time::InSeconds(2.0);
const Time kLoadProblemEndTimeout = Time::InSeconds(1.0);
const Time kShooterBrakeSetTime = Time::InSeconds(0.05);
// Time to wait after releasing the latch piston before winching back again.
diff --git a/y2014/control_loops/shooter/shooter.q b/y2014/control_loops/shooter/shooter.q
index 009e20e..eaaaa2e 100644
--- a/y2014/control_loops/shooter/shooter.q
+++ b/y2014/control_loops/shooter/shooter.q
@@ -49,6 +49,10 @@
// What we think the current position of the hard stop on the shooter is, in
// shot power (Joules).
double hard_stop_power;
+
+ double absolute_position;
+ double absolute_velocity;
+ uint32_t state;
};
queue Goal goal;
diff --git a/y2014/wpilib/wpilib_interface.cc b/y2014/wpilib/wpilib_interface.cc
index f636dee..59cced3 100644
--- a/y2014/wpilib/wpilib_interface.cc
+++ b/y2014/wpilib/wpilib_interface.cc
@@ -252,7 +252,7 @@
::aos::SetCurrentThreadRealtimePriority(kPriority);
while (run_) {
- ::aos::time::PhasedLoopXMS(10, 9000);
+ ::aos::time::PhasedLoopXMS(5, 4000);
RunIteration();
}