Changes to make the robot work with intake and shooter.
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..6efecad 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,11 +83,11 @@
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.
@@ -108,13 +117,13 @@
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)
#pylab.plotfile("shooter.csv", (0,1))
- #pylab.plot(pylab.linspace(0,1.99,200), close_loop_U, 'ro')
+ pylab.plot(pylab.linspace(0,1.99,200), 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,1.99,200), 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 fbfeafb..d1a07f1 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"
@@ -23,7 +24,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;
const double current_position =
@@ -111,6 +112,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 ed42647..56a4f44 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:common',
'<(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 6ab1ea5..7c402b9 100644
--- a/bot3/control_loops/shooter/shooter.h
+++ b/bot3/control_loops/shooter/shooter.h
@@ -1,5 +1,5 @@
-#ifndef FRC971_CONTROL_LOOPS_SHOOTER_H_
-#define FRC971_CONTROL_LOOPS_SHOOTER_H_
+#ifndef BOT3_CONTROL_LOOPS_SHOOTER_H_
+#define BOT3_CONTROL_LOOPS_SHOOTER_H_
#include <memory>
@@ -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:
@@ -54,4 +54,4 @@
} // namespace control_loops
} // namespace bot3
-#endif // FRC971_CONTROL_LOOPS_SHOOTER_H_
+#endif // BOT3_CONTROL_LOOPS_SHOOTER_H_
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/control_loops/shooter/shooter_motor_plant.cc b/bot3/control_loops/shooter/shooter_motor_plant.cc
index 70f052d..5dd32db 100644
--- a/bot3/control_loops/shooter/shooter_motor_plant.cc
+++ b/bot3/control_loops/shooter/shooter_motor_plant.cc
@@ -4,7 +4,7 @@
#include "frc971/control_loops/state_feedback_loop.h"
-namespace frc971 {
+namespace bot3 {
namespace control_loops {
StateFeedbackPlantCoefficients<2, 1, 1> MakeShooterPlantCoefficients() {
diff --git a/bot3/control_loops/shooter/shooter_motor_plant.h b/bot3/control_loops/shooter/shooter_motor_plant.h
index 3588605..2207ab2 100644
--- a/bot3/control_loops/shooter/shooter_motor_plant.h
+++ b/bot3/control_loops/shooter/shooter_motor_plant.h
@@ -1,9 +1,9 @@
-#ifndef FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
+#ifndef BOT3_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
+#define BOT3_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
#include "frc971/control_loops/state_feedback_loop.h"
-namespace frc971 {
+namespace bot3 {
namespace control_loops {
StateFeedbackPlantCoefficients<2, 1, 1> MakeShooterPlantCoefficients();