Merge "Added a start at a fridge."
diff --git a/frc971/control_loops/voltage_cap/voltage_cap.cc b/frc971/control_loops/voltage_cap/voltage_cap.cc
new file mode 100644
index 0000000..3de5828
--- /dev/null
+++ b/frc971/control_loops/voltage_cap/voltage_cap.cc
@@ -0,0 +1,95 @@
+#include "voltage_cap.h"
+
+#include <limits>
+
+namespace frc971 {
+namespace control_loops {
+
+void VoltageCap(double max_voltage, double voltage_one, double voltage_two,
+ double *out_voltage_one, double *out_voltage_two) {
+ *out_voltage_one = *out_voltage_two =
+ ::std::numeric_limits<double>::quiet_NaN();
+ if (voltage_one <= max_voltage && voltage_two <= max_voltage &&
+ voltage_one >= -max_voltage && voltage_two >= -max_voltage) {
+ *out_voltage_one = voltage_one;
+ *out_voltage_two = voltage_two;
+ } else if (voltage_one - voltage_two > 2 * max_voltage) {
+ //(voltage_one is larger)
+ // if the difference between the two voltages is more than 24 we know a
+ // 45degree slope will not intersect the 'box'
+ *out_voltage_one = max_voltage;
+ *out_voltage_two = -max_voltage;
+ // top left corner of 'box'
+ } else if (voltage_one - voltage_two < -2 * max_voltage) {
+ //(voltage_two is larger)
+ // if the difference between the two voltages is less than -24 we know a
+ // 45degree slope will not intersect the 'box'
+ *out_voltage_one = -max_voltage;
+ *out_voltage_two = max_voltage;
+ // bottom right corner of 'box'
+ } else {
+ if (voltage_one >= 0) {
+ if (voltage_two >= 0) {
+ // Quadrant 1
+ if (voltage_one > voltage_two) {
+ // Above box
+ double difference = voltage_one - voltage_two;
+ *out_voltage_one = max_voltage;
+ *out_voltage_two = max_voltage - difference;
+ } else {
+ // Right of box
+ double difference = voltage_two - voltage_one;
+ *out_voltage_one = max_voltage - difference;
+ *out_voltage_two = max_voltage;
+ }
+ } else { // voltage_two < 0
+ // Quadrant 2
+ double difference = voltage_one - voltage_two;
+ if (-voltage_two > voltage_one) {
+ // Left of box
+ *out_voltage_one = -max_voltage + difference;
+ *out_voltage_two = -max_voltage;
+ } else {
+ // Above box
+ *out_voltage_one = max_voltage;
+ *out_voltage_two = max_voltage - difference;
+ }
+ }
+ } else { // voltage_one < 0
+ if (voltage_two <= 0) {
+ // Quadrant 3
+ if (voltage_one < voltage_two) {
+ // Left of Box
+ double difference = voltage_two - voltage_one;
+ *out_voltage_one = -max_voltage;
+ *out_voltage_two = -max_voltage + difference;
+ } else {
+ // Under box
+ double difference = voltage_one - voltage_two;
+ *out_voltage_one = -max_voltage + difference;
+ *out_voltage_two = -max_voltage;
+ }
+ } else { // voltage_two > 0
+ // Quadrant 4
+ double difference = voltage_two - voltage_one;
+ if (-voltage_one > voltage_two) {
+ // Right of box
+ *out_voltage_one = -max_voltage;
+ *out_voltage_two = -max_voltage + difference;
+ } else {
+ // Under box
+ *out_voltage_one = max_voltage - difference;
+ *out_voltage_two = max_voltage;
+ }
+ }
+ }
+ }
+}
+
+void VoltageCap(double voltage_one, double voltage_two, double *out_voltage_one,
+ double *out_voltage_two) {
+ VoltageCap(12.0, voltage_one, voltage_two, out_voltage_one, out_voltage_two);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/voltage_cap/voltage_cap.gyp b/frc971/control_loops/voltage_cap/voltage_cap.gyp
new file mode 100644
index 0000000..08bd27a
--- /dev/null
+++ b/frc971/control_loops/voltage_cap/voltage_cap.gyp
@@ -0,0 +1,23 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'voltage_cap',
+ 'type': 'static_library',
+ 'sources': [
+ 'voltage_cap.cc',
+ ],
+ },
+ {
+ 'target_name': 'voltage_cap_test',
+ 'type': 'executable',
+ 'sources': [
+ 'voltage_cap_test.cc',
+ ],
+ 'dependencies': [
+ 'voltage_cap',
+ '<(EXTERNALS):gtest',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ ],
+ },
+ ],
+}
diff --git a/frc971/control_loops/voltage_cap/voltage_cap.h b/frc971/control_loops/voltage_cap/voltage_cap.h
new file mode 100644
index 0000000..49aa9e8
--- /dev/null
+++ b/frc971/control_loops/voltage_cap/voltage_cap.h
@@ -0,0 +1,33 @@
+#ifndef FRC971_CONTROL_LOOPS_VOLTAGE_CAP_VOLTAGE_CAP_H_
+#define FRC971_CONTROL_LOOPS_VOLTAGE_CAP_VOLTAGE_CAP_H_
+
+#include <stdio.h>
+
+namespace frc971 {
+namespace control_loops {
+
+// This function maintains the difference of power between two voltages passed in
+// that are outside of our range of possible voltage output.
+// This is because we figured that maintaining the difference rather than the ratio
+// between the voltages would get us to our goal as fast as possible.
+//
+//
+// The 'box' is a box formed on a graph by the maximum and minimun voltages of
+// voltage_one and voltage_two with
+// voltage_one on the y-axis and voltage_two on the x-axis.
+// If a line with a slope of one(45degrees) is plotted from the point formed
+// by graphing voltage_one(y) and voltage_two(x), the first intersecting point of
+// the box is the maximum voltage that we can output to get to the goal as
+// fast as possible.
+// If the line does not intersect the box, then we use the closest corner of
+// the box to the line in either quadrant two or quadrant four of the graph.
+void VoltageCap(double max_voltage, double voltage_one, double voltage_two,
+ double *out_voltage_one, double *out_voltage_two);
+ // Defaults to 12v if no voltage is specified.
+void VoltageCap(double voltage_one, double voltage_two, double *out_voltage_one,
+ double *out_voltage_two);
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_VOLTAGE_CAP_VOLTAGE_CAP_H_
diff --git a/frc971/control_loops/voltage_cap/voltage_cap_test.cc b/frc971/control_loops/voltage_cap/voltage_cap_test.cc
new file mode 100644
index 0000000..f508b25
--- /dev/null
+++ b/frc971/control_loops/voltage_cap/voltage_cap_test.cc
@@ -0,0 +1,253 @@
+#include <unistd.h>
+
+#include "frc971/control_loops/voltage_cap/voltage_cap.h"
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+class VoltageTest : public ::testing::Test {
+ protected:
+ // Bring up and down Core.
+ ::aos::common::testing::GlobalCoreInstance my_core;
+};
+
+// Tests that voltage inputs return the same if inside the box.
+TEST_F(VoltageTest, BasicVoltage12) {
+ double voltage_one, voltage_two;
+ VoltageCap(12.0, 11.3, 2.6, &voltage_one, &voltage_two);
+ EXPECT_EQ(11.3, voltage_one);
+ EXPECT_EQ(2.6, voltage_two);
+}
+// Tests that voltage inputs in the 4th quadrant both get capped to their
+// maximum.
+TEST_F(VoltageTest, QuadrantFourNoIntersect12) {
+ double voltage_one, voltage_two;
+ VoltageCap(12.0, -50.0, 50, &voltage_one, &voltage_two);
+ EXPECT_EQ(-12.0, voltage_one);
+ EXPECT_EQ(12.0, voltage_two);
+}
+
+// Tests if the difference between two voltages is more than 24.0v then default
+// to the most outputable voltage in that direction.
+TEST_F(VoltageTest, LargeDifference12) {
+ double voltage_one, voltage_two;
+ VoltageCap(12.0, -13.0, 13.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(-12.0, voltage_one); // just off bottom right corner
+ EXPECT_EQ(12.0, voltage_two);
+
+ VoltageCap(12.0, 13.0, -13.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(12.0, voltage_one); // just off top left corner
+ EXPECT_EQ(-12.0, voltage_two);
+
+ VoltageCap(12.0, 10.0, 36.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(-12.0, voltage_one); // 1st quadrant line just off bottom right
+ EXPECT_EQ(12.0, voltage_two);
+
+ VoltageCap(12.0, 10.0, -36.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(12.0, voltage_one); // 3rd quadrant line just off top left
+ EXPECT_EQ(-12.0, voltage_two);
+}
+
+// Tests that the 45degree angle line intersects the box and returns a value
+// within the box.
+TEST_F(VoltageTest, QuadrantOneIntersect12) {
+ double voltage_one, voltage_two;
+ VoltageCap(12.0, 50.0, 50.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(12.0, voltage_one); // 45degree angle from origin
+ EXPECT_EQ(12.0, voltage_two);
+
+ voltage_one = 0.0;
+
+ VoltageCap(12.0, 13.0, 11.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(12.0, voltage_one);
+ EXPECT_EQ(10.0, voltage_two);
+}
+TEST_F(VoltageTest, QuadrantTwoIntersect12) {
+ double voltage_one, voltage_two;
+ VoltageCap(12.0, 13.0, -2.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(12.0, voltage_one);
+ EXPECT_EQ(-3.0, voltage_two);
+
+ VoltageCap(12.0, 2.0, -13.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(3.0, voltage_one);
+ EXPECT_EQ(-12.0, voltage_two);
+}
+TEST_F(VoltageTest, QuadrantThreeIntersect12) {
+ double voltage_one, voltage_two;
+ VoltageCap(12.0, -50.0, -50.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(-12.0, voltage_one); // 45degree angle from origin
+ EXPECT_EQ(-12.0, voltage_two);
+
+ voltage_one = 0.0;
+
+ VoltageCap(12.0, -13.0, -11.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(-12.0, voltage_one);
+ EXPECT_EQ(-10.0, voltage_two);
+}
+TEST_F(VoltageTest, QuadrantFourIntersect12) {
+ double voltage_one, voltage_two;
+ VoltageCap(12.0, -13.0, 2.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(-12.0, voltage_one);
+ EXPECT_EQ(3.0, voltage_two);
+
+ VoltageCap(12.0, -2.0, 13.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(-3.0, voltage_one);
+ EXPECT_EQ(12.0, voltage_two);
+}
+
+// Tests whether cross quadrants works (also supplies additional points to
+// test).
+TEST_F(VoltageTest, QuadrantOneToTwo12) {
+ // Point in Quadrant 1 intersects box in Quadrant 2.
+ double voltage_one, voltage_two;
+ VoltageCap(12.0, 33.0, 11.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(12.0, voltage_one);
+ EXPECT_EQ(-10.0, voltage_two);
+}
+TEST_F(VoltageTest, QuadrantOneToFour12){
+ // Point in Quadrant 1 intersects box in Quadrant 4.
+ double voltage_one, voltage_two;
+ VoltageCap(12.0, 11.0, 33.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(-10.0, voltage_one);
+ EXPECT_EQ(12.0, voltage_two);
+}
+TEST_F(VoltageTest, QuadrantThreeToTwo12) {
+ // Point in Quadrant 3 intersects box in Quadrant 2.
+ double voltage_one, voltage_two;
+ VoltageCap(12.0, -11.0, -33.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(10.0, voltage_one);
+ EXPECT_EQ(-12.0, voltage_two);
+}
+TEST_F(VoltageTest, QuadrantThreeToFour12) {
+ // Point in Quadrant 3 intersects box in Quadrant 4.
+ double voltage_one, voltage_two;
+ VoltageCap(12.0, -33.0, -11.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(-12.0, voltage_one);
+ EXPECT_EQ(10.0, voltage_two);
+}
+
+// Tests that voltage inputs return the same if inside the box.
+TEST_F(VoltageTest, BasicVoltage6) {
+ double voltage_one, voltage_two;
+ VoltageCap(6.0, 5.3, 2.6, &voltage_one, &voltage_two);
+ EXPECT_EQ(5.3, voltage_one);
+ EXPECT_EQ(2.6, voltage_two);
+}
+// Tests that voltage inputs in the 4th quadrant both get capped to their
+// maximum.
+TEST_F(VoltageTest, QuadrantFourNoIntersect6) {
+ double voltage_one, voltage_two;
+ VoltageCap(6.0, -50.0, 50, &voltage_one, &voltage_two);
+ EXPECT_EQ(-6.0, voltage_one);
+ EXPECT_EQ(6.0, voltage_two);
+}
+
+// Tests if the difference between two voltages is more than 12.0v then default
+// to the most outputable voltage in that direction.
+TEST_F(VoltageTest, LargeDifference6) {
+ double voltage_one, voltage_two;
+ VoltageCap(6.0, -13.0, 13.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(-6.0, voltage_one); // just off bottom right corner
+ EXPECT_EQ(6.0, voltage_two);
+
+ VoltageCap(6.0, 13.0, -13.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(6.0, voltage_one); // just off top left corner
+ EXPECT_EQ(-6.0, voltage_two);
+
+ VoltageCap(6.0, 10.0, 36.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(-6.0, voltage_one); // 1st quadrant line just off bottom right
+ EXPECT_EQ(6.0, voltage_two);
+
+ VoltageCap(6.0, 10.0, -36.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(6.0, voltage_one); // 3rd quadrant line just off top left
+ EXPECT_EQ(-6.0, voltage_two);
+}
+
+// Tests that the 45degree angle line intersects the box and returns a value
+// within the box
+TEST_F(VoltageTest, QuadrantOneIntersect6) {
+ double voltage_one, voltage_two;
+ VoltageCap(6.0, 50.0, 50.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(6.0, voltage_one); // 45degree angle from origin
+ EXPECT_EQ(6.0, voltage_two);
+
+ voltage_one = 0.0;
+
+ VoltageCap(6.0, 13.0, 11.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(6.0, voltage_one);
+ EXPECT_EQ(4.0, voltage_two);
+}
+TEST_F(VoltageTest, QuadrantTwoIntersect6) {
+ double voltage_one, voltage_two;
+ VoltageCap(6.0, 9.0, -2.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(6.0, voltage_one);
+ EXPECT_EQ(-5.0, voltage_two);
+
+ VoltageCap(6.0, 2.0, -9.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(5.0, voltage_one);
+ EXPECT_EQ(-6.0, voltage_two);
+}
+TEST_F(VoltageTest, QuadrantThreeIntersect6) {
+ double voltage_one, voltage_two;
+ VoltageCap(6.0, -50.0, -50.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(-6.0, voltage_one); // 45degree angle from origin
+ EXPECT_EQ(-6.0, voltage_two);
+
+ voltage_one = 0.0;
+
+ VoltageCap(6.0, -13.0, -11.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(-6.0, voltage_one);
+ EXPECT_EQ(-4.0, voltage_two);
+}
+TEST_F(VoltageTest, QuadrantFourIntersect6) {
+ double voltage_one, voltage_two;
+ VoltageCap(6.0, -9.0, 2.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(-6.0, voltage_one);
+ EXPECT_EQ(5.0, voltage_two);
+
+ VoltageCap(6.0, -2.0, 9.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(-5.0, voltage_one);
+ EXPECT_EQ(6.0, voltage_two);
+}
+
+// Tests whether cross quadrants works (also supplies additional points to
+// test).
+TEST_F(VoltageTest, QuadrantOneToTwo6) {
+ // Point in Quadrant 1 intersects box in Quadrant 2.
+ double voltage_one, voltage_two;
+ VoltageCap(6.0, 33.0, 22.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(6.0, voltage_one);
+ EXPECT_EQ(-5.0, voltage_two);
+}
+TEST_F(VoltageTest, QuadrantOneToFour6){
+ // Point in Quadrant 1 intersects box in Quadrant 4.
+ double voltage_one, voltage_two;
+ VoltageCap(6.0, 22.0, 33.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(-5.0, voltage_one);
+ EXPECT_EQ(6.0, voltage_two);
+}
+TEST_F(VoltageTest, QuadrantThreeToTw6) {
+ // Point in Quadrant 3 intersects box in Quadrant 2.
+ double voltage_one, voltage_two;
+ VoltageCap(6.0, -22.0, -33.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(5.0, voltage_one);
+ EXPECT_EQ(-6.0, voltage_two);
+}
+TEST_F(VoltageTest, QuadrantThreeToFour6) {
+ // Point in Quadrant 3 intersects box in Quadrant 4.
+ double voltage_one, voltage_two;
+ VoltageCap(6.0, -33.0, -22.0, &voltage_one, &voltage_two);
+ EXPECT_EQ(-6.0, voltage_one);
+ EXPECT_EQ(5.0, voltage_two);
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index 661a434..c96dc3a 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -18,6 +18,7 @@
'../actions/actions.gyp:drivetrain_action',
'../frc971.gyp:joystick_reader',
'../zeroing/zeroing.gyp:zeroing_test',
+ '../control_loops/voltage_cap/voltage_cap.gyp:voltage_cap_test',
],
'copies': [
{