Merge "Avoid shooter collision with new intake"
diff --git a/aos/common/controls/control_loop_test.cc b/aos/common/controls/control_loop_test.cc
index ecaafab..44af93a 100644
--- a/aos/common/controls/control_loop_test.cc
+++ b/aos/common/controls/control_loop_test.cc
@@ -51,8 +51,8 @@
new_state->voltage_3v3 = 3.3;
new_state->voltage_5v = 5.0;
- new_state->voltage_roborio_in = 12.4;
- new_state->voltage_battery = 12.4;
+ new_state->voltage_roborio_in = battery_voltage_;
+ new_state->voltage_battery = battery_voltage_;
new_state.Send();
}
diff --git a/aos/common/controls/control_loop_test.h b/aos/common/controls/control_loop_test.h
index 4ab4774..14fdd60 100644
--- a/aos/common/controls/control_loop_test.h
+++ b/aos/common/controls/control_loop_test.h
@@ -41,6 +41,11 @@
++reader_pid_;
}
+ // Sets the battery voltage in robot_state.
+ void set_battery_voltage(double battery_voltage) {
+ battery_voltage_ = battery_voltage;
+ }
+
private:
static constexpr ::aos::time::Time kTimeTick = ::aos::time::Time::InUS(5000);
static constexpr ::aos::time::Time kDSPacketTime =
@@ -48,6 +53,7 @@
uint16_t team_id_ = 971;
int32_t reader_pid_ = 1;
+ double battery_voltage_ = 12.4;
::aos::time::Time last_ds_time_ = ::aos::time::Time::InSeconds(0);
::aos::time::Time current_time_ = ::aos::time::Time::InSeconds(0);
diff --git a/frc971/analysis/analysis.py b/frc971/analysis/analysis.py
index 2dafb27..8817a26 100755
--- a/frc971/analysis/analysis.py
+++ b/frc971/analysis/analysis.py
@@ -1,8 +1,11 @@
#!/usr/bin/python3
+
+import collections
import matplotlib
from matplotlib import pylab
from matplotlib.font_manager import FontProperties
-import collections
+import re
+import sys
class Dataset(object):
def __init__(self):
@@ -134,34 +137,43 @@
self.HandleLine(line)
+"""
+A regular expression to match the envelope part of the log entry.
+Parsing of the JSON msg is handled elsewhere.
+"""
+LOG_RE = re.compile("""
+ (.*?) # 1 name
+ \((\d+)\) # 2 pid
+ \((\d+)\) # 3 message_index
+ :\s
+ (\w+?) # 4 level
+ \s+at\s+
+ (\d+\.\d+)s # 5 time
+ :\s
+ ([A-Za-z0-9_./-]+) # 6 filename
+ :\s
+ (\d+) # 7 linenumber
+ :\s
+ (.*) # 8 msg
+ """, re.VERBOSE)
+
class LogEntry:
"""This class provides a way to parse log entries."""
def __init__(self, line):
- """Creates a LogEntry from a line."""
- name_index = line.find('(')
- self.name = line[0:name_index]
-
- pid_index = line.find(')', name_index + 1)
- self.pid = int(line[name_index + 1:pid_index])
-
- msg_index_index = line.find(')', pid_index + 1)
- self.msg_index = int(line[pid_index + 2:msg_index_index])
-
- level_index = line.find(' ', msg_index_index + 3)
- self.level = line[msg_index_index + 3:level_index]
-
- time_index_start = line.find(' at ', level_index) + 4
- time_index_end = line.find('s:', level_index)
- self.time = float(line[time_index_start:time_index_end])
-
- filename_end = line.find(':', time_index_end + 3)
- self.filename = line[time_index_end + 3:filename_end]
-
- linenumber_end = line.find(':', filename_end + 2)
- self.linenumber = int(line[filename_end + 2:linenumber_end])
-
- self.msg = line[linenumber_end+2:]
+ """Populates a LogEntry from a line."""
+ m = LOG_RE.match(line)
+ if m is None:
+ print("LOG_RE failed on", line)
+ sys.exit(1)
+ self.name = m.group(1)
+ self.pid_index = int(m.group(2))
+ self.msg_index = int(m.group(3))
+ self.level = m.group(4)
+ self.time = float(m.group(5))
+ self.filename = m.group(6)
+ self.linenumber = m.group(7)
+ self.msg = m.group(8)
def __str__(self):
"""Formats the data cleanly."""
diff --git a/y2015/control_loops/claw/BUILD b/y2015/control_loops/claw/BUILD
index e671004..200b422 100644
--- a/y2015/control_loops/claw/BUILD
+++ b/y2015/control_loops/claw/BUILD
@@ -25,17 +25,41 @@
],
)
+genrule(
+ name = 'genrule_claw',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2015/control_loops/python:claw) $(OUTS)',
+ tools = [
+ '//y2015/control_loops/python:claw',
+ ],
+ outs = [
+ 'claw_motor_plant.h',
+ 'claw_motor_plant.cc',
+ ],
+)
+
cc_library(
- name = 'claw_lib',
+ name = 'claw_plants',
srcs = [
- 'claw.cc',
'claw_motor_plant.cc',
],
hdrs = [
- 'claw.h',
'claw_motor_plant.h',
],
deps = [
+ '//frc971/control_loops:state_feedback_loop',
+ ],
+)
+
+cc_library(
+ name = 'claw_lib',
+ hdrs = [
+ 'claw.h',
+ ],
+ srcs = [
+ 'claw.cc',
+ ],
+ deps = [
':claw_queue',
'//aos/common/controls:control_loop',
'//aos/common:time',
@@ -43,6 +67,7 @@
'//y2015:constants',
'//frc971/control_loops:state_feedback_loop',
'//frc971/zeroing:zeroing',
+ ':claw_plants',
],
)
diff --git a/y2015/control_loops/claw/claw.cc b/y2015/control_loops/claw/claw.cc
index e487024..e6ab9e7 100644
--- a/y2015/control_loops/claw/claw.cc
+++ b/y2015/control_loops/claw/claw.cc
@@ -33,7 +33,8 @@
Claw::Claw(control_loops::ClawQueue *claw)
: aos::controls::ControlLoop<control_loops::ClawQueue>(claw),
last_piston_edge_(Time::Now()),
- claw_loop_(new ClawCappedStateFeedbackLoop(MakeClawLoop())),
+ claw_loop_(new ClawCappedStateFeedbackLoop(
+ ::y2015::control_loops::claw::MakeClawLoop())),
claw_estimator_(constants::GetValues().claw.zeroing),
profile_(::aos::controls::kLoopFrequency) {}
diff --git a/y2015/control_loops/claw/claw_lib_test.cc b/y2015/control_loops/claw/claw_lib_test.cc
index 10fcc07..6b3ad87 100644
--- a/y2015/control_loops/claw/claw_lib_test.cc
+++ b/y2015/control_loops/claw/claw_lib_test.cc
@@ -25,7 +25,8 @@
public:
// Constructs a claw simulation.
ClawSimulation()
- : claw_plant_(new StateFeedbackPlant<2, 1, 1>(MakeClawPlant())),
+ : claw_plant_(new StateFeedbackPlant<2, 1, 1>(
+ y2015::control_loops::claw::MakeClawPlant())),
pot_and_encoder_(constants::GetValues().claw.zeroing.index_difference),
claw_queue_(".frc971.control_loops.claw_queue", 0x9d7452fb,
".frc971.control_loops.claw_queue.goal",
diff --git a/y2015/control_loops/claw/claw_motor_plant.cc b/y2015/control_loops/claw/claw_motor_plant.cc
deleted file mode 100644
index 5f5c206..0000000
--- a/y2015/control_loops/claw/claw_motor_plant.cc
+++ /dev/null
@@ -1,49 +0,0 @@
-#include "y2015/control_loops/claw/claw_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeClawPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00482455476758, 0.0, 0.930652495326;
- Eigen::Matrix<double, 2, 1> B;
- B << 6.97110924671e-05, 0.0275544125308;
- Eigen::Matrix<double, 1, 2> C;
- C << 1, 0;
- Eigen::Matrix<double, 1, 1> D;
- D << 0;
- Eigen::Matrix<double, 1, 1> U_max;
- U_max << 12.0;
- Eigen::Matrix<double, 1, 1> U_min;
- U_min << -12.0;
- return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<2, 1, 1> MakeClawController() {
- Eigen::Matrix<double, 2, 1> L;
- L << 0.998251427366, 26.9874526231;
- Eigen::Matrix<double, 1, 2> K;
- K << 74.4310031124, 4.72251126222;
- Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.0, -0.00518405612386, 0.0, 1.07451492907;
- return StateFeedbackController<2, 1, 1>(L, K, A_inv, MakeClawPlantCoefficients());
-}
-
-StateFeedbackPlant<2, 1, 1> MakeClawPlant() {
- ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 1, 1>>> plants(1);
- plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 1, 1>>(new StateFeedbackPlantCoefficients<2, 1, 1>(MakeClawPlantCoefficients()));
- return StateFeedbackPlant<2, 1, 1>(&plants);
-}
-
-StateFeedbackLoop<2, 1, 1> MakeClawLoop() {
- ::std::vector< ::std::unique_ptr<StateFeedbackController<2, 1, 1>>> controllers(1);
- controllers[0] = ::std::unique_ptr<StateFeedbackController<2, 1, 1>>(new StateFeedbackController<2, 1, 1>(MakeClawController()));
- return StateFeedbackLoop<2, 1, 1>(&controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/y2015/control_loops/claw/claw_motor_plant.h b/y2015/control_loops/claw/claw_motor_plant.h
deleted file mode 100644
index 6cb9cdb..0000000
--- a/y2015/control_loops/claw/claw_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef Y2015_CONTROL_LOOPS_CLAW_CLAW_MOTOR_PLANT_H_
-#define Y2015_CONTROL_LOOPS_CLAW_CLAW_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeClawPlantCoefficients();
-
-StateFeedbackController<2, 1, 1> MakeClawController();
-
-StateFeedbackPlant<2, 1, 1> MakeClawPlant();
-
-StateFeedbackLoop<2, 1, 1> MakeClawLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // Y2015_CONTROL_LOOPS_CLAW_CLAW_MOTOR_PLANT_H_
diff --git a/y2015/control_loops/python/BUILD b/y2015/control_loops/python/BUILD
new file mode 100644
index 0000000..ab66d3e
--- /dev/null
+++ b/y2015/control_loops/python/BUILD
@@ -0,0 +1,25 @@
+package(default_visibility = ['//y2015:__subpackages__'])
+
+py_binary(
+ name = 'arm',
+ srcs = [
+ 'arm.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ],
+)
+
+py_binary(
+ name = 'claw',
+ srcs = [
+ 'claw.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ],
+)
diff --git a/y2015/control_loops/python/claw.py b/y2015/control_loops/python/claw.py
index cdb76ae..86a261d 100755
--- a/y2015/control_loops/python/claw.py
+++ b/y2015/control_loops/python/claw.py
@@ -1,13 +1,21 @@
#!/usr/bin/python
-import control_loop
-import controls
-import polytope
-import polydrivetrain
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+from frc971.control_loops.python import polytope
import numpy
import sys
import matplotlib
from matplotlib import pylab
+import glog
+import gflags
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
class Claw(control_loop.ControlLoop):
def __init__(self, name="Claw", mass=None):
@@ -110,7 +118,7 @@
def run_test(claw, initial_X, goal, max_separation_error=0.01,
- show_graph=True, iterations=200, controller_claw=None,
+ show_graph=False, iterations=200, controller_claw=None,
observer_claw=None):
"""Runs the claw plant with an initial condition and goal.
@@ -198,14 +206,13 @@
# Write the generated constants out to a file.
if len(argv) != 3:
- print "Expected .h file name and .cc file name for the claw."
+ glog.fatal('Expected .h and .cc filename for claw.')
else:
- claw = Claw("Claw")
- loop_writer = control_loop.ControlLoopWriter("Claw", [claw])
- if argv[1][-3:] == '.cc':
- loop_writer.Write(argv[2], argv[1])
- else:
- loop_writer.Write(argv[1], argv[2])
+ namespaces = ['y2015', 'control_loops', 'claw']
+ claw = Claw('Claw')
+ loop_writer = control_loop.ControlLoopWriter('Claw', [claw],
+ namespaces=namespaces)
+ loop_writer.Write(argv[1], argv[2])
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/y2016/constants.h b/y2016/constants.h
index 98d38c7..d9ae7c4 100644
--- a/y2016/constants.h
+++ b/y2016/constants.h
@@ -35,7 +35,7 @@
static constexpr double kIntakeEncoderRatio =
16.0 / 48.0 * 18.0 / 72.0 * 14.0 / 64.0;
static constexpr double kShoulderEncoderRatio =
- 16.0 / 58.0 * 18.0 / 72.0 * 14.0 / 64.0;
+ 12.0 / 42.0 * 18.0 / 72.0 * 14.0 / 64.0;
static constexpr double kWristEncoderRatio =
16.0 / 48.0 * 18.0 / 64.0 * 14.0 / 54.0;
diff --git a/y2016/control_loops/python/intake.py b/y2016/control_loops/python/intake.py
index 1fcf393..63cc0f4 100755
--- a/y2016/control_loops/python/intake.py
+++ b/y2016/control_loops/python/intake.py
@@ -40,10 +40,9 @@
# Gear ratio
self.G = (56.0 / 12.0) * (54.0 / 14.0) * (64.0 / 18.0) * (48.0 / 16.0)
- # Measured in CAD
- # self.J = 0.9
- # With extra mass to compensate for friction.
- self.J = 1.2
+ # Moment of inertia, measured in CAD.
+ # Extra mass to compensate for friction is added on.
+ self.J = 0.34 + 0.1
# Control loop time step
self.dt = 0.005
diff --git a/y2016/control_loops/python/shoulder.py b/y2016/control_loops/python/shoulder.py
index 9b70dbd..07a67f6 100755
--- a/y2016/control_loops/python/shoulder.py
+++ b/y2016/control_loops/python/shoulder.py
@@ -37,7 +37,7 @@
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratio
- self.G = (56.0 / 12.0) * (64.0 / 14.0) * (72.0 / 18.0) * (58.0 / 16.0)
+ self.G = (56.0 / 12.0) * (64.0 / 14.0) * (72.0 / 18.0) * (42.0 / 12.0)
self.J = 3.0
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index ba49675..8147fc2 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -14,8 +14,6 @@
namespace superstructure {
namespace {
-constexpr double kZeroingVoltage = 5.0;
-constexpr double kOperatingVoltage = 12.0;
constexpr double kLandingShoulderDownVoltage = -2.0;
// The maximum voltage the intake roller will be allowed to use.
constexpr float kMaxIntakeTopVoltage = 12.0;
@@ -644,6 +642,8 @@
last_state_ = state_before_switch;
}
+constexpr double Superstructure::kZeroingVoltage;
+constexpr double Superstructure::kOperatingVoltage;
constexpr double Superstructure::kShoulderMiddleAngle;
constexpr double Superstructure::kLooseTolerance;
constexpr double Superstructure::kIntakeUpperClear;
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index b7cfbd4..083b5b8 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -101,6 +101,9 @@
control_loops::SuperstructureQueue *my_superstructure =
&control_loops::superstructure_queue);
+ static constexpr double kZeroingVoltage = 5.0;
+ static constexpr double kOperatingVoltage = 12.0;
+
// This is the angle above which we will do a HIGH_ARM_ZERO, and below which
// we will do a LOW_ARM_ZERO.
static constexpr double kShoulderMiddleAngle = M_PI / 4.0;
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index bc33c9b..2c2bf5a 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -170,18 +170,18 @@
superstructure_queue_.status->state ==
Superstructure::LANDING_RUNNING) {
CHECK_LE(::std::abs(superstructure_queue_.output->voltage_intake),
- 12.00001);
+ Superstructure::kOperatingVoltage);
CHECK_LE(::std::abs(superstructure_queue_.output->voltage_shoulder),
- 12.00001);
+ Superstructure::kOperatingVoltage);
CHECK_LE(::std::abs(superstructure_queue_.output->voltage_wrist),
- 12.00001);
+ Superstructure::kOperatingVoltage);
} else {
CHECK_LE(::std::abs(superstructure_queue_.output->voltage_intake),
- 4.00001);
+ Superstructure::kZeroingVoltage);
CHECK_LE(::std::abs(superstructure_queue_.output->voltage_shoulder),
- 4.00001);
+ Superstructure::kZeroingVoltage);
CHECK_LE(::std::abs(superstructure_queue_.output->voltage_wrist),
- 4.00001);
+ Superstructure::kZeroingVoltage);
}
if (arm_plant_->X(0, 0) <=
Superstructure::kShoulderTransitionToLanded + 1e-4) {
@@ -960,7 +960,7 @@
.max_angular_acceleration_wrist(100)
.Send());
- set_peak_intake_velocity(4.60);
+ set_peak_intake_velocity(4.65);
set_peak_shoulder_velocity(1.00);
set_peak_wrist_velocity(1.00);
RunForTime(Time::InSeconds(4));