Tuned intake.
Change-Id: I028f6cbb8df55d281734ca39abb4f62b7fd27793
diff --git a/y2018/constants.h b/y2018/constants.h
index ce09903..4416abc 100644
--- a/y2018/constants.h
+++ b/y2018/constants.h
@@ -40,8 +40,8 @@
kDrivetrainEncoderCountsPerRevolution();
}
- static constexpr double kDrivetrainShifterPotMaxVoltage() { return 1.94; }
- static constexpr double kDrivetrainShifterPotMinVoltage() { return 3.63; }
+ static constexpr double kDrivetrainShifterPotMaxVoltage() { return 3.63; }
+ static constexpr double kDrivetrainShifterPotMinVoltage() { return 1.94; }
static constexpr double kProximalEncoderCountsPerRevolution() { return 4096.0; }
static constexpr double kProximalEncoderRatio() {
diff --git a/y2018/control_loops/python/intake.py b/y2018/control_loops/python/intake.py
index 1a7df6a..8879dff 100755
--- a/y2018/control_loops/python/intake.py
+++ b/y2018/control_loops/python/intake.py
@@ -12,214 +12,229 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
+
class Intake(control_loop.ControlLoop):
- def __init__(self, name="Intake"):
- super(Intake, self).__init__(name)
- self.motor = control_loop.BAG()
- # TODO(constants): Update all of these & retune poles.
- # Stall Torque in N m
- self.stall_torque = self.motor.stall_torque
- # Stall Current in Amps
- self.stall_current = self.motor.stall_current
- # Free Speed in RPM
- self.free_speed = self.motor.free_speed
- # Free Current in Amps
- self.free_current = self.motor.free_current
+ def __init__(self, name="Intake"):
+ super(Intake, self).__init__(name)
+ self.motor = control_loop.BAG()
+ # Stall Torque in N m
+ self.stall_torque = self.motor.stall_torque
+ # Stall Current in Amps
+ self.stall_current = self.motor.stall_current
+ # Free Speed in RPM
+ self.free_speed = self.motor.free_speed
+ # Free Current in Amps
+ self.free_current = self.motor.free_current
- # Resistance of the motor
- self.resistance = self.motor.resistance
- # Motor velocity constant
- self.Kv = self.motor.Kv
- # Torque constant
- self.Kt = self.motor.Kt
- # Gear ratio
- self.G = 1.0 / 100.0
+ # Resistance of the motor
+ self.resistance = self.motor.resistance
+ # Motor velocity constant
+ self.Kv = self.motor.Kv
+ # Torque constant
+ self.Kt = self.motor.Kt
+ # Gear ratio
+ self.G = 1.0 / 102.6
- self.motor_inertia = 0.000006
+ self.motor_inertia = 0.00000589 * 1.2
- # Series elastic moment of inertia
- self.Je = self.motor_inertia / (self.G * self.G)
- # Grabber moment of inertia
- self.Jo = 0.295
+ # Series elastic moment of inertia
+ self.Je = self.motor_inertia / (self.G * self.G)
+ # Grabber moment of inertia
+ self.Jo = 0.0363
- # Spring constant (N m / radian)
- self.Ks = 30.0
+ # Bot has a time constant of 0.22
+ # Current physics has a time constant of 0.18
- # Control loop time step
- self.dt = 0.00505
+ # Spring constant (N m / radian)
+ self.Ks = 32.74
- # State is [output_position, output_velocity,
- # elastic_position, elastic_velocity]
- # The output position is the absolute position of the intake arm.
- # The elastic position is the absolute position of the motor side of the
- # series elastic.
- # Input is [voltage]
+ # Control loop time step
+ self.dt = 0.00505
- self.A_continuous = numpy.matrix(
- [[0.0, 1.0, 0.0, 0.0],
- [(-self.Ks / self.Jo), 0.0, (self.Ks / self.Jo), 0.0],
- [0.0, 0.0, 0.0, 1.0],
- [(self.Ks / self.Je), 0.0, (-self.Ks / self.Je), \
- -self.Kt / (self.Je * self.resistance * self.Kv * self.G * self.G)]])
+ # State is [output_position, output_velocity,
+ # elastic_position, elastic_velocity]
+ # The output position is the absolute position of the intake arm.
+ # The elastic position is the absolute position of the motor side of the
+ # series elastic.
+ # Input is [voltage]
- # Start with the unmodified input
- self.B_continuous = numpy.matrix(
- [[0.0],
- [0.0],
- [0.0],
- [self.Kt / (self.G * self.Je * self.resistance)]])
+ self.A_continuous = numpy.matrix(
+ [[0.0, 1.0, 0.0, 0.0],
+ [(-self.Ks / self.Jo), 0.0, (self.Ks / self.Jo), 0.0],
+ [0.0, 0.0, 0.0, 1.0],
+ [(self.Ks / self.Je), 0.0, (-self.Ks / self.Je), \
+ -self.Kt / (self.Je * self.resistance * self.Kv * self.G * self.G)]])
- self.C = numpy.matrix([[1.0, 0.0, -1.0, 0.0],
- [0.0, 0.0, 1.0, 0.0]])
- self.D = numpy.matrix([[0.0],
- [0.0]])
+ # Start with the unmodified input
+ self.B_continuous = numpy.matrix(
+ [[0.0], [0.0], [0.0],
+ [self.Kt / (self.G * self.Je * self.resistance)]])
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C = numpy.matrix([[1.0, 0.0, -1.0, 0.0], [0.0, 0.0, 1.0, 0.0]])
+ self.D = numpy.matrix([[0.0], [0.0]])
- controllability = controls.ctrb(self.A, self.B)
- glog.debug('ctrb: ' + repr(numpy.linalg.matrix_rank(controllability)))
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- observability = controls.ctrb(self.A.T, self.C.T)
- glog.debug('obs: ' + repr(numpy.linalg.matrix_rank(observability)))
+ #controllability = controls.ctrb(self.A, self.B)
+ #glog.debug('ctrb: ' + repr(numpy.linalg.matrix_rank(controllability)))
- glog.debug('A_continuous ' + repr(self.A_continuous))
- glog.debug('B_continuous ' + repr(self.B_continuous))
+ #observability = controls.ctrb(self.A.T, self.C.T)
+ #glog.debug('obs: ' + repr(numpy.linalg.matrix_rank(observability)))
- self.K = numpy.matrix(numpy.zeros((1, 4)))
+ glog.debug('A_continuous ' + repr(self.A_continuous))
+ glog.debug('B_continuous ' + repr(self.B_continuous))
- q_pos = 0.05
- q_vel = 2.65
- self.Q = numpy.matrix(numpy.diag([(q_pos ** 2.0), (q_vel ** 2.0),
- (q_pos ** 2.0), (q_vel ** 2.0)]))
+ self.K = numpy.matrix(numpy.zeros((1, 4)))
- r_nm = 0.025
- self.R = numpy.matrix(numpy.diag([(r_nm ** 2.0), (r_nm ** 2.0)]))
+ q_pos = 0.05
+ q_vel = 2.65
+ self.Q = numpy.matrix(
+ numpy.diag([(q_pos**2.0), (q_vel**2.0), (q_pos**2.0), (q_vel
+ **2.0)]))
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ r_nm = 0.025
+ self.R = numpy.matrix(numpy.diag([(r_nm**2.0), (r_nm**2.0)]))
- # 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]])
- self.U_min = numpy.matrix([[-12.0]])
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.InitializeState()
+ # 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]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
class DelayedIntake(Intake):
- def __init__(self, name="DelayedIntake"):
- super(DelayedIntake, self).__init__(name=name)
+ def __init__(self, name="DelayedIntake"):
+ super(DelayedIntake, self).__init__(name=name)
- self.A_undelayed = self.A
- self.B_undelayed = self.B
+ self.A_undelayed = self.A
+ self.B_undelayed = self.B
- self.C_unaugmented = self.C
- self.C = numpy.matrix(numpy.zeros((2, 5)))
- self.C[0:2, 0:4] = self.C_unaugmented
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((2, 5)))
+ self.C[0:2, 0:4] = self.C_unaugmented
- # Model this as X[4] is the last power. And then B applies to the last
- # power. This lets us model the 1 cycle PWM delay accurately.
- self.A = numpy.matrix(numpy.zeros((5, 5)))
- self.A[0:4, 0:4] = self.A_undelayed
- self.A[0:4, 4] = self.B_undelayed
- self.B = numpy.matrix(numpy.zeros((5, 1)))
- self.B[4, 0] = 1.0
+ # Model this as X[4] is the last power. And then B applies to the last
+ # power. This lets us model the 1 cycle PWM delay accurately.
+ self.A = numpy.matrix(numpy.zeros((5, 5)))
+ self.A[0:4, 0:4] = self.A_undelayed
+ self.A[0:4, 4] = self.B_undelayed
+ self.B = numpy.matrix(numpy.zeros((5, 1)))
+ self.B[4, 0] = 1.0
- # Coordinate transform fom absolute angles to relative angles.
- # [output_position, output_velocity, spring_angle, spring_velocity, voltage]
- abs_to_rel = numpy.matrix([[ 1.0, 0.0, 0.0, 0.0, 0.0],
- [ 0.0, 1.0, 0.0, 0.0, 0.0],
- [-1.0, 0.0, 1.0, 0.0, 0.0],
- [ 0.0, -1.0, 0.0, 1.0, 0.0],
- [ 0.0, 0.0, 0.0, 0.0, 1.0]])
- # and back again.
- rel_to_abs = numpy.matrix(numpy.linalg.inv(abs_to_rel))
+ # Coordinate transform fom absolute angles to relative angles.
+ # [output_position, output_velocity, spring_angle, spring_velocity, voltage]
+ abs_to_rel = numpy.matrix(
+ [[1.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 1.0, 0.0, 0.0, 0.0],
+ [1.0, 0.0, -1.0, 0.0, 0.0],
+ [0.0, 1.0, 0.0, -1.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 1.0]])
+ # and back again.
+ rel_to_abs = numpy.matrix(numpy.linalg.inv(abs_to_rel))
- # Now, get A and B in the relative coordinate system.
- self.A_transformed_full = abs_to_rel * self.A * rel_to_abs
- self.B_transformed_full = abs_to_rel * self.B
+ # Now, get A and B in the relative coordinate system.
+ self.A_transformed_full = numpy.matrix(numpy.zeros((5, 5)))
+ self.B_transformed_full = numpy.matrix(numpy.zeros((5, 1)))
+ (self.A_transformed_full[0:4, 0:4],
+ self.A_transformed_full[0:4, 4]) = self.ContinuousToDiscrete(
+ abs_to_rel[0:4, 0:4] * self.A_continuous * rel_to_abs[0:4, 0:4],
+ abs_to_rel[0:4, 0:4] * self.B_continuous, self.dt)
+ self.B_transformed_full[4, 0] = 1.0
- # Pull out the components of the dynamics which don't include the spring
- # output positoin so we can do partial state feedback on what we care about.
- self.A_transformed = self.A_transformed_full[1:5, 1:5]
- self.B_transformed = self.B_transformed_full[1:5, 0]
+ # Pull out the components of the dynamics which don't include the spring
+ # output position so we can do partial state feedback on what we care about.
+ self.A_transformed = self.A_transformed_full[1:5, 1:5]
+ self.B_transformed = self.B_transformed_full[1:5, 0]
- glog.debug('A_transformed_full ' + str(self.A_transformed_full))
- glog.debug('B_transformed_full ' + str(self.B_transformed_full))
- glog.debug('A_transformed ' + str(self.A_transformed))
- glog.debug('B_transformed ' + str(self.B_transformed))
+ glog.debug('A_transformed_full ' + str(self.A_transformed_full))
+ glog.debug('B_transformed_full ' + str(self.B_transformed_full))
+ glog.debug('A_transformed ' + str(self.A_transformed))
+ glog.debug('B_transformed ' + str(self.B_transformed))
- # Now, let's design a controller in
- # [output_velocity, spring_position, spring_velocity, delayed_voltage]
- # space.
+ # Now, let's design a controller in
+ # [output_velocity, spring_position, spring_velocity, delayed_voltage]
+ # space.
- q_output_vel = 0.20
- q_spring_pos = 0.05
- q_spring_vel = 3.0
- q_voltage = 100.0
- self.Q_lqr = numpy.matrix(numpy.diag(
- [1.0 / (q_output_vel ** 2.0),
- 1.0 / (q_spring_pos ** 2.0),
- 1.0 / (q_spring_vel ** 2.0),
- 1.0 / (q_voltage ** 2.0)]))
+ q_output_vel = 1.0
+ q_spring_pos = 0.5
+ q_spring_vel = 2.0
+ q_voltage = 1000000000000.0
+ self.Q_lqr = numpy.matrix(
+ numpy.diag([
+ 1.0 / (q_output_vel**2.0), 1.0 / (q_spring_pos**2.0),
+ 1.0 / (q_spring_vel**2.0), 1.0 / (q_voltage**2.0)
+ ]))
- self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
+ self.R = numpy.matrix([[(1.0 / (12.0**2.0))]])
- self.K_transformed = controls.dlqr(self.A_transformed, self.B_transformed,
- self.Q_lqr, self.R)
+ self.K_transformed = controls.dlqr(
+ self.A_transformed, self.B_transformed, self.Q_lqr, self.R)
- # Write the controller back out in the absolute coordinate system.
- self.K = numpy.hstack((numpy.matrix([[0.0]]),
- self.K_transformed)) * abs_to_rel
+ # Write the controller back out in the absolute coordinate system.
+ self.K = numpy.hstack((numpy.matrix([[0.0]]),
+ self.K_transformed)) * abs_to_rel
- glog.debug('Poles are %s for %s',
- repr(numpy.linalg.eig(
- self.A_transformed -
- self.B_transformed * self.K_transformed)[0]), self._name)
- glog.debug('K is %s', repr(self.K_transformed))
+ controllability = controls.ctrb(self.A_transformed, self.B_transformed)
+ glog.debug('ctrb: ' + repr(numpy.linalg.matrix_rank(controllability)))
- # Design a kalman filter here as well.
- q_pos = 0.05
- q_vel = 2.65
- q_volts = 0.005
- self.Q = numpy.matrix(numpy.diag([(q_pos ** 2.0), (q_vel ** 2.0),
- (q_pos ** 2.0), (q_vel ** 2.0),
- (q_volts ** 2.0)]))
+ w, v = numpy.linalg.eig(
+ self.A_transformed - self.B_transformed * self.K_transformed)
+ glog.debug('Poles are %s, for %s', repr(w), self._name)
- r_nm = 0.025
- self.R = numpy.matrix(numpy.diag([(r_nm ** 2.0), (r_nm ** 2.0)]))
+ for i in range(len(w)):
+ glog.debug(' Pole %s -> %s', repr(w[i]), v[:, i])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ glog.debug('K is %s', repr(self.K_transformed))
- # 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]])
- self.U_min = numpy.matrix([[-12.0]])
+ # Design a kalman filter here as well.
+ q_pos = 0.05
+ q_vel = 2.65
+ q_volts = 0.005
+ self.Q = numpy.matrix(
+ numpy.diag([(q_pos**2.0), (q_vel**2.0), (q_pos**2.0), (q_vel**2.0),
+ (q_volts**2.0)]))
- self.InitializeState()
+ r_nm = 0.025
+ self.R = numpy.matrix(numpy.diag([(r_nm**2.0), (r_nm**2.0)]))
+
+ glog.debug('Overall poles are %s, for %s',
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[0]),
+ self._name)
+
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+
+ self.InitializeState()
class ScenarioPlotter(object):
- def __init__(self):
- # Various lists for graphing things.
- self.t = []
- self.x = []
- self.v = []
- self.goal_v = []
- self.a = []
- self.spring = []
- self.x_hat = []
- self.u = []
+ def __init__(self):
+ # Various lists for graphing things.
+ self.t = []
+ self.x_motor = []
+ self.x_output = []
+ self.v = []
+ self.goal_v = []
+ self.a = []
+ self.spring = []
+ self.x_hat = []
+ self.u = []
- def run_test(self, intake, iterations=400, controller_intake=None,
- observer_intake=None):
- """Runs the intake plant with an initial condition and goal.
+ def run_test(self,
+ intake,
+ iterations=400,
+ controller_intake=None,
+ observer_intake=None):
+ """Runs the intake plant with an initial condition and goal.
Test for whether the goal has been reached and whether the separation
goes outside of the initial and goal values by more than
@@ -236,127 +251,137 @@
should use the actual state.
"""
- if controller_intake is None:
- controller_intake = intake
+ if controller_intake is None:
+ controller_intake = intake
- vbat = 12.0
+ vbat = 12.0
- if self.t:
- initial_t = self.t[-1] + intake.dt
- else:
- initial_t = 0
+ if self.t:
+ initial_t = self.t[-1] + intake.dt
+ else:
+ initial_t = 0
- # Delay U by 1 cycle in our simulation to make it more realistic.
- last_U = numpy.matrix([[0.0]])
+ # Delay U by 1 cycle in our simulation to make it more realistic.
+ last_U = numpy.matrix([[0.0]])
+ intake.Y = intake.C * intake.X
- for i in xrange(iterations):
- X_hat = intake.X
+ for i in xrange(iterations):
+ X_hat = intake.X
- if observer_intake is not None:
- X_hat = observer_intake.X_hat
- self.x_hat.append(observer_intake.X_hat[0, 0])
+ if observer_intake is not None:
+ X_hat = observer_intake.X_hat
+ self.x_hat.append(observer_intake.X_hat[0, 0])
- goal_angle = 3.0
- goal_velocity = numpy.clip((goal_angle - X_hat[0, 0]) * 6.0, -10.0, 10.0)
+ goal_angle = 3.0
+ goal_velocity = numpy.clip((goal_angle - X_hat[0, 0]) * 6.0, -1.0,
+ 1.0)
- self.goal_v.append(goal_velocity)
+ self.goal_v.append(goal_velocity)
- # Nominal: 1.8 N at 0.25 m -> 0.45 N m
- # Nominal: 13 N at 0.25 m at 0.5 radians -> 3.25 N m -> 6 N m / radian
+ # Nominal: 1.8 N at 0.25 m -> 0.45 N m
+ # Nominal: 13 N at 0.25 m at 0.5 radians -> 3.25 N m -> 6 N m / radian
- R = numpy.matrix([[0.0],
- [goal_velocity],
- [0.0],
- [goal_velocity],
- [goal_velocity / (intake.G * intake.Kv)]])
- U = controller_intake.K * (R - X_hat) + R[4, 0]
+ R = numpy.matrix([[0.0], [goal_velocity], [0.0], [goal_velocity],
+ [goal_velocity / (intake.G * intake.Kv)]])
+ U = controller_intake.K * (R - X_hat) + R[4, 0]
- U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat) # * 0.0
- self.x.append(intake.X[0, 0])
- self.spring.append((intake.X[2, 0] - intake.X[0, 0]) * intake.Ks)
+ self.x_output.append(intake.X[0, 0])
+ self.x_motor.append(intake.X[2, 0])
+ self.spring.append(intake.X[0, 0] - intake.X[2, 0])
- if self.v:
- last_v = self.v[-1]
- else:
- last_v = 0
+ if self.v:
+ last_v = self.v[-1]
+ else:
+ last_v = 0
- self.v.append(intake.X[1, 0])
- self.a.append((self.v[-1] - last_v) / intake.dt)
+ self.v.append(intake.X[1, 0])
+ self.a.append((self.v[-1] - last_v) / intake.dt)
- if observer_intake is not None:
- observer_intake.Y = intake.Y
- observer_intake.CorrectObserver(U)
+ if observer_intake is not None:
+ observer_intake.Y = intake.Y
+ observer_intake.CorrectObserver(U)
- intake.Update(last_U + 0.0)
+ intake.Update(last_U + 0.0)
- if observer_intake is not None:
- observer_intake.PredictObserver(U)
+ if observer_intake is not None:
+ observer_intake.PredictObserver(U)
- self.t.append(initial_t + i * intake.dt)
- self.u.append(U[0, 0])
- last_U = U
+ self.t.append(initial_t + i * intake.dt)
+ self.u.append(U[0, 0])
+ last_U = U
- def Plot(self):
- pylab.subplot(3, 1, 1)
- pylab.plot(self.t, self.x, label='x')
- pylab.plot(self.t, self.x_hat, label='x_hat')
- pylab.legend()
+ def Plot(self):
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.t, self.x_output, label='x output')
+ pylab.plot(self.t, self.x_motor, label='x motor')
+ pylab.plot(self.t, self.x_hat, label='x_hat')
+ pylab.legend()
- spring_ax1 = pylab.subplot(3, 1, 2)
- spring_ax1.plot(self.t, self.u, 'k', label='u')
- spring_ax2 = spring_ax1.twinx()
- spring_ax2.plot(self.t, self.spring, label='spring_angle')
- spring_ax1.legend(loc=2)
- spring_ax2.legend()
+ spring_ax1 = pylab.subplot(3, 1, 2)
+ spring_ax1.plot(self.t, self.u, 'k', label='u')
+ spring_ax2 = spring_ax1.twinx()
+ spring_ax2.plot(self.t, self.spring, label='spring_angle')
+ spring_ax1.legend(loc=2)
+ spring_ax2.legend()
- accel_ax1 = pylab.subplot(3, 1, 3)
- accel_ax1.plot(self.t, self.a, 'r', label='a')
+ accel_ax1 = pylab.subplot(3, 1, 3)
+ accel_ax1.plot(self.t, self.a, 'r', label='a')
- accel_ax2 = accel_ax1.twinx()
- accel_ax2.plot(self.t, self.v, label='v')
- accel_ax2.plot(self.t, self.goal_v, label='goal_v')
- accel_ax1.legend(loc=2)
- accel_ax2.legend()
+ accel_ax2 = accel_ax1.twinx()
+ accel_ax2.plot(self.t, self.v, label='v')
+ accel_ax2.plot(self.t, self.goal_v, label='goal_v')
+ accel_ax1.legend(loc=2)
+ accel_ax2.legend()
- pylab.show()
+ pylab.show()
def main(argv):
- scenario_plotter = ScenarioPlotter()
+ scenario_plotter = ScenarioPlotter()
- intake = Intake()
- intake_controller = DelayedIntake()
- observer_intake = DelayedIntake()
+ intake = Intake()
+ intake.X[0, 0] = 0.0
+ intake_controller = DelayedIntake()
+ observer_intake = DelayedIntake()
+ observer_intake.X_hat[0, 0] = intake.X[0, 0]
- # Test moving the intake with constant separation.
- scenario_plotter.run_test(intake, controller_intake=intake_controller,
- observer_intake=observer_intake, iterations=200)
+ # Test moving the intake with constant separation.
+ scenario_plotter.run_test(
+ intake,
+ controller_intake=intake_controller,
+ observer_intake=observer_intake,
+ iterations=200)
- if FLAGS.plot:
- scenario_plotter.Plot()
+ 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 intake and delayed_intake.')
- else:
- namespaces = ['y2018', 'control_loops', 'superstructure', 'intake']
- intake = Intake('Intake')
- loop_writer = control_loop.ControlLoopWriter(
- 'Intake', [intake], namespaces=namespaces)
- loop_writer.AddConstant(control_loop.Constant('kGearRatio', '%f', intake.G))
- loop_writer.AddConstant(
- control_loop.Constant('kMotorVelocityConstant', '%f', intake.Kv))
- loop_writer.AddConstant(
- control_loop.Constant('kFreeSpeed', '%f', intake.free_speed))
- loop_writer.Write(argv[1], argv[2])
+ # Write the generated constants out to a file.
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for intake and delayed_intake.'
+ )
+ else:
+ namespaces = ['y2018', 'control_loops', 'superstructure', 'intake']
+ intake = Intake('Intake')
+ loop_writer = control_loop.ControlLoopWriter(
+ 'Intake', [intake], namespaces=namespaces)
+ loop_writer.AddConstant(
+ control_loop.Constant('kGearRatio', '%f', intake.G))
+ loop_writer.AddConstant(
+ control_loop.Constant('kMotorVelocityConstant', '%f', intake.Kv))
+ loop_writer.AddConstant(
+ control_loop.Constant('kFreeSpeed', '%f', intake.free_speed))
+ loop_writer.Write(argv[1], argv[2])
- delayed_intake = DelayedIntake('DelayedIntake')
- loop_writer = control_loop.ControlLoopWriter(
- 'DelayedIntake', [delayed_intake], namespaces=namespaces)
- loop_writer.Write(argv[3], argv[4])
+ delayed_intake = DelayedIntake('DelayedIntake')
+ loop_writer = control_loop.ControlLoopWriter(
+ 'DelayedIntake', [delayed_intake], namespaces=namespaces)
+ loop_writer.Write(argv[3], argv[4])
+
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2018/control_loops/superstructure/intake/intake.cc b/y2018/control_loops/superstructure/intake/intake.cc
index 7212fd3..080720a 100644
--- a/y2018/control_loops/superstructure/intake/intake.cc
+++ b/y2018/control_loops/superstructure/intake/intake.cc
@@ -71,7 +71,7 @@
goal_velocity = 0.0;
} else {
goal_velocity = ::aos::Clip(
- ((goal_angle(unsafe_goal) - loop_->X_hat(0, 0)) * 6.0), -10.0, 10.0);
+ ((goal_angle(unsafe_goal) - loop_->X_hat(0, 0)) * 12.0), -16.0, 16.0);
}
// Computes the goal.
loop_->mutable_R() << 0.0, goal_velocity, 0.0, goal_velocity,
@@ -84,8 +84,8 @@
const double *unsafe_goal) {
status->goal_position = goal_angle(unsafe_goal);
status->goal_velocity = loop_->R(1, 0);
- status->spring_position = loop_->X_hat(0);
- status->spring_velocity = loop_->X_hat(1);
+ status->spring_position = loop_->X_hat(0) - loop_->X_hat(2);
+ status->spring_velocity = loop_->X_hat(1) - loop_->X_hat(3);
status->motor_position = loop_->X_hat(2);
status->motor_velocity = loop_->X_hat(3);
status->delayed_voltage = loop_->X_hat(4);
@@ -102,7 +102,6 @@
const control_loops::IntakeElasticSensors *position,
control_loops::IntakeVoltage *output,
control_loops::IntakeSideStatus *status) {
- double intake_last_position_ = status->estimator_state.position;
zeroing_estimator_.UpdateEstimate(position->motor_position);
switch (state_) {
@@ -164,6 +163,7 @@
status->zeroed = zeroing_estimator_.zeroed();
status->estopped = (state_ == State::ESTOP);
status->state = static_cast<int32_t>(state_);
+ intake_last_position_ = status->estimator_state.position;
}
} // namespace intake
diff --git a/y2018/control_loops/superstructure/intake/intake.h b/y2018/control_loops/superstructure/intake/intake.h
index d46d90e..b154334 100644
--- a/y2018/control_loops/superstructure/intake/intake.h
+++ b/y2018/control_loops/superstructure/intake/intake.h
@@ -97,6 +97,8 @@
State state_ = State::UNINITIALIZED;
::frc971::zeroing::PotAndAbsEncoderZeroingEstimator zeroing_estimator_;
+
+ double intake_last_position_ = 0.0;
};
} // namespace intake
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index f53a6db..1c5a4e5 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -63,12 +63,66 @@
status->arm.zeroed;
if (output && unsafe_goal) {
- output->intake.left.voltage_rollers = ::std::max(
+ const double roller_voltage = ::std::max(
-kMaxIntakeRollerVoltage, ::std::min(unsafe_goal->intake.roller_voltage,
kMaxIntakeRollerVoltage));
- output->intake.right.voltage_rollers = ::std::max(
- -kMaxIntakeRollerVoltage, ::std::min(unsafe_goal->intake.roller_voltage,
- kMaxIntakeRollerVoltage));
+ constexpr int kReverseTime = 15;
+ if (unsafe_goal->intake.roller_voltage < 0.0) {
+ output->intake.left.voltage_rollers = roller_voltage;
+ output->intake.right.voltage_rollers = roller_voltage;
+ rotation_state_ = RotationState::NOT_ROTATING;
+ rotation_count_ = 0;
+ } else {
+ switch (rotation_state_) {
+ case RotationState::NOT_ROTATING:
+ if (position->intake.left.beam_break) {
+ rotation_state_ = RotationState::ROTATING_RIGHT;
+ rotation_count_ = kReverseTime;
+ break;
+ } else if (position->intake.right.beam_break) {
+ rotation_state_ = RotationState::ROTATING_LEFT;
+ rotation_count_ = kReverseTime;
+ break;
+ } else {
+ break;
+ }
+ case RotationState::ROTATING_LEFT:
+ if (position->intake.right.beam_break) {
+ rotation_count_ = kReverseTime;
+ } else {
+ --rotation_count_;
+ }
+ if (rotation_count_ == 0) {
+ rotation_state_ = RotationState::NOT_ROTATING;
+ }
+ break;
+ case RotationState::ROTATING_RIGHT:
+ if (position->intake.left.beam_break) {
+ rotation_count_ = kReverseTime;
+ } else {
+ --rotation_count_;
+ }
+ if (rotation_count_ == 0) {
+ rotation_state_ = RotationState::NOT_ROTATING;
+ }
+ break;
+ }
+
+ switch (rotation_state_) {
+ case RotationState::NOT_ROTATING:
+ output->intake.left.voltage_rollers = roller_voltage;
+ output->intake.right.voltage_rollers = roller_voltage;
+ break;
+ case RotationState::ROTATING_LEFT:
+ output->intake.left.voltage_rollers = roller_voltage;
+ output->intake.right.voltage_rollers = -roller_voltage;
+ break;
+ case RotationState::ROTATING_RIGHT:
+ output->intake.left.voltage_rollers = -roller_voltage;
+ output->intake.right.voltage_rollers = roller_voltage;
+ break;
+ }
+ }
}
}
diff --git a/y2018/control_loops/superstructure/superstructure.h b/y2018/control_loops/superstructure/superstructure.h
index ade5047..2081e6c 100644
--- a/y2018/control_loops/superstructure/superstructure.h
+++ b/y2018/control_loops/superstructure/superstructure.h
@@ -36,6 +36,15 @@
intake::IntakeSide intake_right_;
arm::Arm arm_;
+ enum class RotationState {
+ NOT_ROTATING = 0,
+ ROTATING_LEFT = 1,
+ ROTATING_RIGHT = 2
+ };
+
+ RotationState rotation_state_ = RotationState::NOT_ROTATING;
+ int rotation_count_ = 0;
+
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
index 09809e6..601cec3 100644
--- a/y2018/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -279,14 +279,16 @@
ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
// Left side test.
EXPECT_NEAR(superstructure_queue_.goal->intake.left_intake_angle,
- superstructure_queue_.status->left_intake.spring_position,
+ superstructure_queue_.status->left_intake.spring_position +
+ superstructure_queue_.status->left_intake.motor_position,
0.001);
EXPECT_NEAR(superstructure_queue_.goal->intake.left_intake_angle,
superstructure_plant_.left_intake().spring_position(), 0.001);
// Right side test.
EXPECT_NEAR(superstructure_queue_.goal->intake.right_intake_angle,
- superstructure_queue_.status->right_intake.spring_position,
+ superstructure_queue_.status->right_intake.spring_position +
+ superstructure_queue_.status->right_intake.motor_position,
0.001);
EXPECT_NEAR(superstructure_queue_.goal->intake.right_intake_angle,
superstructure_plant_.right_intake().spring_position(), 0.001);
@@ -395,12 +397,17 @@
// Check that we are near our soft limit.
superstructure_queue_.status.FetchLatest();
+ EXPECT_NEAR(0.0, superstructure_queue_.status->left_intake.spring_position,
+ 0.001);
EXPECT_NEAR(constants::Values::kIntakeRange().upper,
- superstructure_queue_.status->left_intake.spring_position,
+ superstructure_queue_.status->left_intake.spring_position +
+ superstructure_queue_.status->left_intake.motor_position,
0.001);
+ EXPECT_NEAR(0.0, superstructure_queue_.status->right_intake.spring_position,
+ 0.001);
EXPECT_NEAR(constants::Values::kIntakeRange().upper,
- superstructure_queue_.status->right_intake.spring_position,
+ superstructure_queue_.status->right_intake.motor_position,
0.001);
// Set some ridiculous goals to test lower limits.
@@ -419,12 +426,15 @@
superstructure_queue_.status.FetchLatest();
EXPECT_NEAR(constants::Values::kIntakeRange().lower,
- superstructure_queue_.status->left_intake.spring_position,
+ superstructure_queue_.status->left_intake.motor_position, 0.001);
+ EXPECT_NEAR(0.0, superstructure_queue_.status->left_intake.spring_position,
0.001);
EXPECT_NEAR(constants::Values::kIntakeRange().lower,
- superstructure_queue_.status->right_intake.spring_position,
- 0.001);}
+ superstructure_queue_.status->right_intake.motor_position, 0.001);
+ EXPECT_NEAR(0.0, superstructure_queue_.status->right_intake.spring_position,
+ 0.001);
+}
TEST_F(SuperstructureTest, DISABLED_LowerHardstopStartup) {
superstructure_plant_.InitializeIntakePosition(
diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index def6df1..067b59f 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -28,10 +28,10 @@
namespace input {
namespace joysticks {
-const ButtonLocation kIntakeDown(3, 9);
+const ButtonLocation kIntakeDown(3, 7);
const POVLocation kIntakeUp(3, 90);
-const ButtonLocation kIntakeIn(3, 12);
-const ButtonLocation kIntakeOut(3, 8);
+const POVLocation kIntakeIn(3, 270);
+const ButtonLocation kIntakeOut(3, 6);
const ButtonLocation kArmDown(3, 12);
const ButtonLocation kArmSwitch(3, 8);
@@ -94,11 +94,11 @@
if (data.IsPressed(kIntakeUp)) {
// Bring in the intake.
- intake_goal_ = -M_PI; //TODO(Neil): Add real value here once we have one.
+ intake_goal_ = -M_PI * 2.0 / 3.0;
}
if (data.IsPressed(kIntakeDown)) {
// Deploy the intake.
- intake_goal_ = 0; //TODO(Neil): Add real value here once we have one.
+ intake_goal_ = 0.25;
}
auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
@@ -108,12 +108,10 @@
if (data.IsPressed(kIntakeIn)) {
// Turn on the rollers.
- new_superstructure_goal->intake.roller_voltage =
- 9.0; //TODO(Neil): Add real value once we have one.
+ new_superstructure_goal->intake.roller_voltage = 8.0;
} else if (data.IsPressed(kIntakeOut)) {
// Turn off the rollers.
- new_superstructure_goal->intake.roller_voltage =
- -9.0; //TODO(Neil): Add real value once we have one.
+ new_superstructure_goal->intake.roller_voltage = -12.0;
} else {
// We don't want the rollers on.
new_superstructure_goal->intake.roller_voltage = 0.0;
@@ -159,7 +157,7 @@
}
// Current goals to send to the robot.
- double intake_goal_ = 0.0;
+ double intake_goal_ = -M_PI * 2.0 / 3.0;
bool was_running_ = false;
bool auto_running_ = false;
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index 64f2ccf..dcbe314 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -721,7 +721,7 @@
12.0);
left_intake_rollers_victor_->SetSpeed(
- ::aos::Clip(-queue->intake.right.voltage_rollers, -kMaxBringupPower,
+ ::aos::Clip(-queue->intake.left.voltage_rollers, -kMaxBringupPower,
kMaxBringupPower) /
12.0);