Changes to make the robot work with intake and shooter.
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..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();
diff --git a/bot3/input/gyro_sensor_receiver.cc b/bot3/input/gyro_sensor_receiver.cc
index 5b31505..31394da 100644
--- a/bot3/input/gyro_sensor_receiver.cc
+++ b/bot3/input/gyro_sensor_receiver.cc
@@ -52,10 +52,10 @@
.angle(data()->gyro_angle / 16.0 / 1000.0 / 180.0 * M_PI)
.Send();
- drivetrain.position.MakeWithBuilder()
- .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();*/
LOG(DEBUG, "right: %lf left: %lf angle: %lld \n",
drivetrain_translate(data()->main.wrist),
drivetrain_translate(data()->main.shooter), data()->gyro_angle);
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..6922e1e 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,23 @@
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 = 250;
+ }
+ if (data.IsPressed(kIntake)) {
+ intake = 0.9;
+ }
+ 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..5ac1bd2 100644
--- a/bot3/output/atom_motor_writer.cc
+++ b/bot3/output/atom_motor_writer.cc
@@ -47,9 +47,13 @@
shooter.output.FetchLatest();
if (shooter.output.IsNewerThanMS(kOutputMaxAgeMS)) {
- SetPWMOutput(2, shooter.output->voltage / 12.0, kVictorBounds);
+ SetPWMOutput(1, 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.