Merge in the changes James made at NASA.
This is the code that got the robot driving originally.
Conflicts:
bot3/control_loops/shooter/shooter_motor_plant.cc
bot3/input/gyro_sensor_receiver.cc
diff --git a/aos/common/control_loop/ControlLoop-tmpl.h b/aos/common/control_loop/ControlLoop-tmpl.h
index 69c3121..5d22300 100644
--- a/aos/common/control_loop/ControlLoop-tmpl.h
+++ b/aos/common/control_loop/ControlLoop-tmpl.h
@@ -53,16 +53,16 @@
if (!control_loop_->position.IsNewerThanMS(kPositionTimeoutMs)) {
LOG(ERROR, "Stale position. %d ms > %d ms. Outputs disabled.\n",
msec_age, kPositionTimeoutMs);
- ZeroOutputs();
- return;
+ //ZeroOutputs();
+ //eturn;
} else {
LOG(ERROR, "Stale position. %d ms\n", msec_age);
}
} else {
LOG(ERROR, "Never had a position.\n");
if (fail_no_position) {
- ZeroOutputs();
- return;
+ // ZeroOutputs();
+ // return;
}
}
}
diff --git a/bot3/control_loops/python/control_loop.py b/bot3/control_loops/python/control_loop.py
index 754ba62..5ff724b 100644
--- a/bot3/control_loops/python/control_loop.py
+++ b/bot3/control_loops/python/control_loop.py
@@ -17,7 +17,7 @@
if namespaces:
self._namespaces = namespaces
else:
- self._namespaces = ['frc971', 'control_loops']
+ self._namespaces = ['bot3', 'control_loops']
self._namespace_start = '\n'.join(
['namespace %s {' % name for name in self._namespaces])
@@ -26,7 +26,7 @@
['} // namespace %s' % name for name in reversed(self._namespaces)])
def _HeaderGuard(self, header_file):
- return ('FRC971_CONTROL_LOOPS_' +
+ return ('BOT3_CONTROL_LOOPS_' +
header_file.upper().replace('.', '_').replace('/', '_') +
'_')
diff --git a/bot3/control_loops/python/shooter.py b/bot3/control_loops/python/shooter.py
index cc7930f..60b3219 100755
--- a/bot3/control_loops/python/shooter.py
+++ b/bot3/control_loops/python/shooter.py
@@ -4,6 +4,7 @@
import sys
from matplotlib import pylab
import control_loop
+import slycot
class Shooter(control_loop.ControlLoop):
def __init__(self):
@@ -43,9 +44,17 @@
self.A, self.B = self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
self.dt)
+ R_LQR = numpy.matrix([[1e-5]])
+ P = slycot.sb02od(2,1, self.A, self.B, self.C.T * self.C, R_LQR, 'D')[0]
+
+ self.K = numpy.linalg.inv(R_LQR + self.B.T * P * self.B) * self.B.T * P * self.A
+
+ print self.K
+
self.InitializeState()
- self.PlaceControllerPoles([.6, .981])
+# self.PlaceControllerPoles([.6, .981])
+# print self.K
self.rpl = .45
self.ipl = 0.07
@@ -74,22 +83,22 @@
last_x = shooter_data[i, 2]
sim_delay = 1
- pylab.plot(range(sim_delay, shooter_data.shape[0] + sim_delay),
- simulated_x, label='Simulation')
- pylab.plot(range(shooter_data.shape[0]), real_x, label='Reality')
- pylab.plot(range(shooter_data.shape[0]), x_vel, label='Velocity')
- pylab.legend()
+# pylab.plot(range(sim_delay, shooter_data.shape[0] + sim_delay),
+# simulated_x, label='Simulation')
+# pylab.plot(range(shooter_data.shape[0]), real_x, label='Reality')
+# pylab.plot(range(shooter_data.shape[0]), x_vel, label='Velocity')
+# pylab.legend()
# pylab.show()
# Simulate the closed loop response of the system to a step input.
shooter = Shooter()
close_loop_x = []
close_loop_U = []
- velocity_goal = 300
+ velocity_goal = 350
R = numpy.matrix([[0.0], [velocity_goal]])
- for _ in pylab.linspace(0,1.99,200):
+ for _ in pylab.linspace(0,3.99,400):
# Iterate the position up.
- R = numpy.matrix([[R[0, 0] + 10.5], [velocity_goal]])
+ R = numpy.matrix([[R[0, 0] + velocity_goal / 100], [velocity_goal]])
# Prevents the position goal from going beyond what is necessary.
velocity_weight_scalar = 0.35
max_reference = (
@@ -108,13 +117,14 @@
shooter.UpdateObserver(U)
shooter.Update(U)
close_loop_x.append(shooter.X[1, 0])
- close_loop_U.append(U[0, 0])
+ close_loop_U.append(U[0, 0] * 10)
+ print U[0, 0]
#pylab.plotfile("shooter.csv", (0,1))
- #pylab.plot(pylab.linspace(0,1.99,200), close_loop_U, 'ro')
+ pylab.plot(pylab.linspace(0,3.99,400), close_loop_U)
#pylab.plotfile("shooter.csv", (0,2))
- pylab.plot(pylab.linspace(0,1.99,200), close_loop_x, 'ro')
-# pylab.show()
+ pylab.plot(pylab.linspace(0,3.99,400), close_loop_x)
+ pylab.show()
# Simulate spin down.
spin_down_x = [];
diff --git a/bot3/control_loops/shooter/shooter.cc b/bot3/control_loops/shooter/shooter.cc
index 07ce397..fb78a10 100644
--- a/bot3/control_loops/shooter/shooter.cc
+++ b/bot3/control_loops/shooter/shooter.cc
@@ -1,4 +1,5 @@
#include "bot3/control_loops/shooter/shooter.h"
+#include "bot3/control_loops/shooter/shooter_motor.q.h"
#include "aos/common/control_loop/control_loops.q.h"
#include "aos/common/logging/logging.h"
@@ -20,7 +21,7 @@
void ShooterMotor::RunIteration(
const control_loops::ShooterLoop::Goal *goal,
const control_loops::ShooterLoop::Position *position,
- ::aos::control_loops::Output *output,
+ control_loops::ShooterLoop::Output *output,
control_loops::ShooterLoop::Status *status) {
double velocity_goal = goal->velocity;
// Our position here is actually a velocity.
@@ -72,6 +73,9 @@
if (output) {
output->voltage = output_voltage;
+ output->intake = goal->intake;
+ output->push = goal->push;
+ LOG(DEBUG, "goal: %lf, volt: %lf, push:%d\n", goal->intake, output_voltage, goal->push);
}
}
diff --git a/bot3/control_loops/shooter/shooter.gyp b/bot3/control_loops/shooter/shooter.gyp
index 28e6b14..2be0439 100644
--- a/bot3/control_loops/shooter/shooter.gyp
+++ b/bot3/control_loops/shooter/shooter.gyp
@@ -29,7 +29,7 @@
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- #'<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
],
'export_dependent_settings': [
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
diff --git a/bot3/control_loops/shooter/shooter.h b/bot3/control_loops/shooter/shooter.h
index efc1f1b..16176de 100644
--- a/bot3/control_loops/shooter/shooter.h
+++ b/bot3/control_loops/shooter/shooter.h
@@ -28,7 +28,7 @@
virtual void RunIteration(
const control_loops::ShooterLoop::Goal *goal,
const control_loops::ShooterLoop::Position *position,
- ::aos::control_loops::Output *output,
+ control_loops::ShooterLoop::Output *output,
control_loops::ShooterLoop::Status *status);
private:
diff --git a/bot3/control_loops/shooter/shooter_motor.q b/bot3/control_loops/shooter/shooter_motor.q
index 87eaa16..714e77a 100644
--- a/bot3/control_loops/shooter/shooter_motor.q
+++ b/bot3/control_loops/shooter/shooter_motor.q
@@ -8,6 +8,10 @@
message Goal {
// Goal velocity in rad/sec
double velocity;
+ // PWM output to intake.
+ double intake;
+ // Whether to activate pusher piston.
+ bool push;
};
message Status {
@@ -15,6 +19,8 @@
bool ready;
// The average velocity over the last 0.1 seconds.
double average_velocity;
+ // True if Frisbees are being fired into the shooter.
+ bool firing;
};
message Position {
@@ -22,9 +28,18 @@
double position;
};
+ message Output {
+ // Output to shooter, Volts.
+ double voltage;
+ // PWM output to intake.
+ double intake;
+ // Whether to activate pusher piston.
+ bool push;
+ };
+
queue Goal goal;
queue Position position;
- queue aos.control_loops.Output output;
+ queue Output output;
queue Status status;
};
diff --git a/bot3/input/gyro_sensor_receiver.cc b/bot3/input/gyro_sensor_receiver.cc
index bb6bac7..bd36865 100644
--- a/bot3/input/gyro_sensor_receiver.cc
+++ b/bot3/input/gyro_sensor_receiver.cc
@@ -68,6 +68,10 @@
.right_encoder(drivetrain_translate(data()->main.wrist))
.left_encoder(drivetrain_translate(data()->main.shooter))
.Send();
+ /*drivetrain.position.MakeWithBuilder()
+ .right_encoder(0)
+ .left_encoder(0)
+ .Send();*/
shooter.position.MakeWithBuilder()
.position(shooter_translate(data()->bot3.shooter_cycle_ticks));
diff --git a/bot3/input/input.gyp b/bot3/input/input.gyp
index 3e1cfa7..b43fc48 100644
--- a/bot3/input/input.gyp
+++ b/bot3/input/input.gyp
@@ -12,6 +12,7 @@
'<(AOS)/build/aos.gyp:logging',
'<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/bot3/control_loops/shooter/shooter.gyp:shooter_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(DEPTH)/bot3/autonomous/autonomous.gyp:auto_queue',
],
diff --git a/bot3/input/joystick_reader.cc b/bot3/input/joystick_reader.cc
index 5efe8b4..199d1e6 100644
--- a/bot3/input/joystick_reader.cc
+++ b/bot3/input/joystick_reader.cc
@@ -8,12 +8,14 @@
#include "aos/common/logging/logging.h"
#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/control_loops/shooter/shooter_motor.q.h"
#include "bot3/autonomous/auto.q.h"
#include "frc971/queues/GyroAngle.q.h"
#include "frc971/queues/Piston.q.h"
#include "frc971/queues/CameraTarget.q.h"
using ::bot3::control_loops::drivetrain;
+using ::bot3::control_loops::shooter;
using ::frc971::control_loops::shifters;
using ::frc971::sensors::gyro;
// using ::frc971::vision::target_angle;
@@ -32,15 +34,10 @@
const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
const ButtonLocation kQuickTurn(1, 5);
-const ButtonLocation kLongShot(3, 5);
-const ButtonLocation kMediumShot(3, 3);
-const ButtonLocation kShortShot(3, 6);
-const ButtonLocation kPitShot1(2, 7), kPitShot2(2, 10);
+const ButtonLocation kPush(3, 9);
-const ButtonLocation kFire(3, 11);
-const ButtonLocation kIntake(3, 10);
-const ButtonLocation kForceFire(3, 12);
-const ButtonLocation kForceIndexUp(3, 9), kForceIndexDown(3, 7);
+const ButtonLocation kFire(3, 3);
+const ButtonLocation kIntake(3, 4);
class Reader : public ::aos::input::JoystickInput {
public:
@@ -126,6 +123,41 @@
if (data.PosEdge(kShiftLow)) {
is_high_gear = true;
}
+
+ // 3, 4, 9, 9 fires, 3 pickups
+
+ shooter.status.FetchLatest();
+ bool push = false;
+ double velocity = 0.0;
+ double intake = 0.0;
+ if (data.IsPressed(kPush) && shooter.status->ready) {
+ push = true;
+ }
+ if (data.IsPressed(kFire)) {
+ velocity = 500;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 1))) {
+ velocity = 50;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 2))) {
+ velocity = 250;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 5))) {
+ velocity = 300;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 7))) {
+ velocity = 350;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 8))) {
+ velocity = 400;
+ }
+ else if (data.IsPressed(ButtonLocation(3, 10))) {
+ velocity = 450;
+ }
+ if (data.IsPressed(kIntake)) {
+ intake = 0.8;
+ }
+ shooter.goal.MakeWithBuilder().intake(intake).velocity(velocity).push(push).Send();
#if 0
::aos::ScopedMessagePtr<frc971::control_loops::ShooterLoop::Goal> shooter_goal =
shooter.goal.MakeMessage();
diff --git a/bot3/output/atom_motor_writer.cc b/bot3/output/atom_motor_writer.cc
index 43a45a3..a16dfc4 100644
--- a/bot3/output/atom_motor_writer.cc
+++ b/bot3/output/atom_motor_writer.cc
@@ -47,13 +47,83 @@
shooter.output.FetchLatest();
if (shooter.output.IsNewerThanMS(kOutputMaxAgeMS)) {
- SetPWMOutput(2, shooter.output->voltage / 12.0, kVictorBounds);
+ SetPWMOutput(1, LinearizeVictor(shooter.output->voltage / 12.0), kVictorBounds);
+ SetPWMOutput(7, shooter.output->intake, kVictorBounds);
+ SetSolenoid(4, shooter.output->push);
+ LOG(DEBUG, "shooter: %lf, intake: %lf, push: %d", shooter.output->voltage, shooter.output->intake, shooter.output->push);
} else {
DisablePWMOutput(2);
+ DisablePWMOutput(1);
LOG(WARNING, "shooter not new enough\n");
}
// TODO(danielp): Add stuff for intake and shooter.
}
+
+ // This linearizes the value from the victor.
+ // Copied form the 2012 code.
+ double LinearizeVictor(double goal_speed) {
+ // These values were derived by putting the robot up on blocks, and driving it
+ // at various speeds. The power required to drive at these speeds was then
+ // recorded and fit with gnuplot.
+ const double deadband_value = 0.082;
+ // If we are outside the range such that the motor is actually moving,
+ // subtract off the constant offset from the deadband. This makes the
+ // function odd and intersect the origin, making the fitting easier to do.
+ if (goal_speed > deadband_value) {
+ goal_speed -= deadband_value;
+ } else if (goal_speed < -deadband_value) {
+ goal_speed += deadband_value;
+ } else {
+ goal_speed = 0.0;
+ }
+ goal_speed = goal_speed / (1.0 - deadband_value);
+
+ double goal_speed2 = goal_speed * goal_speed;
+ double goal_speed3 = goal_speed2 * goal_speed;
+ double goal_speed4 = goal_speed3 * goal_speed;
+ double goal_speed5 = goal_speed4 * goal_speed;
+ double goal_speed6 = goal_speed5 * goal_speed;
+ double goal_speed7 = goal_speed6 * goal_speed;
+
+ // Constants for the 5th order polynomial
+ double victor_fit_e1 = 0.437239;
+ double victor_fit_c1 = -1.56847;
+ double victor_fit_a1 = (- (125.0 * victor_fit_e1 + 125.0
+ * victor_fit_c1 - 116.0) / 125.0);
+ double answer_5th_order = (victor_fit_a1 * goal_speed5
+ + victor_fit_c1 * goal_speed3
+ + victor_fit_e1 * goal_speed);
+
+ // Constants for the 7th order polynomial
+ double victor_fit_c2 = -5.46889;
+ double victor_fit_e2 = 2.24214;
+ double victor_fit_g2 = -0.042375;
+ double victor_fit_a2 = (- (125.0 * (victor_fit_c2 + victor_fit_e2
+ + victor_fit_g2) - 116.0) / 125.0);
+ double answer_7th_order = (victor_fit_a2 * goal_speed7
+ + victor_fit_c2 * goal_speed5
+ + victor_fit_e2 * goal_speed3
+ + victor_fit_g2 * goal_speed);
+
+
+ // Average the 5th and 7th order polynomials, and add a bit of linear power in
+ // as well. The average turns out to nicely fit the data in gnuplot with nice
+ // smooth curves, and the linear power gives it a bit more punch at low speeds
+ // again. Stupid victors.
+ double answer = 0.85 * 0.5 * (answer_7th_order + answer_5th_order)
+ + .15 * goal_speed * (1.0 - deadband_value);
+
+ // Add the deadband power back in to make it so that the motor starts moving
+ // when any power is applied. This is what the fitting assumes.
+ if (answer > 0.001) {
+ answer += deadband_value;
+ } else if (answer < -0.001) {
+ answer -= deadband_value;
+ }
+
+ return answer;
+ }
+
};
} // namespace output