This stuff lets tests run
diff --git a/aos/linux_code/configuration.cc b/aos/linux_code/configuration.cc
index 55e5263..a8cc4ae 100644
--- a/aos/linux_code/configuration.cc
+++ b/aos/linux_code/configuration.cc
@@ -26,12 +26,15 @@
static const char *kOverrideVariable = "FRC971_IP_OVERRIDE";
const char *override_ip = getenv(kOverrideVariable);
if (override_ip != NULL) {
+ LOG(INFO, "Override IP is %s\n", override_ip);
static in_addr r;
if (inet_aton(override_ip, &r) != 0) {
return &r;
} else {
LOG(WARNING, "error parsing %s value '%s'\n", kOverrideVariable, override_ip);
}
+ } else {
+ LOG(INFO, "Couldn't get environmental variable.\n");
}
ifaddrs *addrs;
diff --git a/frc971/constants.cc b/frc971/constants.cc
index ad5e191..b17a7cb 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -38,6 +38,12 @@
0.47};
const ShifterHallEffect kPracticeRightDriveShifter{2.070124, 0.838993, 0.62,
0.55};
+const double shooter_lower_limit=0.0;
+const double shooter_upper_limit=0.0;
+const double shooter_hall_effect_start_position=0.0;
+const double shooter_zeroing_off_speed=0.0;
+const double shooter_zeroing_speed=0.0;
+const double pos=0.0;
const Values *DoGetValues() {
uint16_t team = ::aos::network::GetTeamNumber();
@@ -48,6 +54,12 @@
kCompDrivetrainEncoderRatio,
kCompLowGearRatio,
kCompHighGearRatio,
+ shooter_lower_limit,
+ shooter_upper_limit,
+ shooter_hall_effect_start_position,
+ shooter_zeroing_off_speed,
+ shooter_zeroing_speed,
+ pos,
kCompLeftDriveShifter,
kCompRightDriveShifter,
true,
@@ -60,6 +72,12 @@
kPracticeDrivetrainEncoderRatio,
kPracticeLowGearRatio,
kPracticeHighGearRatio,
+ shooter_lower_limit,
+ shooter_upper_limit,
+ shooter_hall_effect_start_position,
+ shooter_zeroing_off_speed,
+ shooter_zeroing_speed,
+ pos,
kPracticeLeftDriveShifter,
kPracticeRightDriveShifter,
false,
diff --git a/frc971/constants.h b/frc971/constants.h
index 2d12bfa..58825ca 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -32,6 +32,13 @@
// gear.
double low_gear_ratio;
double high_gear_ratio;
+
+ double shooter_lower_limit;
+ double shooter_upper_limit;
+ double shooter_hall_effect_start_position;
+ double shooter_zeroing_off_speed;
+ double shooter_zeroing_speed;
+ double pos;
ShifterHallEffect left_drive, right_drive;
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
index 11699ac..bb88523 100755
--- a/frc971/control_loops/python/shooter.py
+++ b/frc971/control_loops/python/shooter.py
@@ -132,17 +132,17 @@
pylab.show()
# Write the generated constants out to a file.
- if len(argv) != 3:
+ if len(argv) != 5:
print "Expected .h file name and .cc file name for"
print "both the plant and unaugmented plant"
else:
unaug_shooter = Shooter("RawShooter")
unaug_loop_writer = control_loop.ControlLoopWriter("RawShooter",
[unaug_shooter])
- #if argv[3][-3:] == '.cc':
- # unaug_loop_writer.Write(argv[4], argv[3])
- #else:
- # unaug_loop_writer.Write(argv[3], argv[4])
+ if argv[3][-3:] == '.cc':
+ unaug_loop_writer.Write(argv[4], argv[3])
+ else:
+ unaug_loop_writer.Write(argv[3], argv[4])
loop_writer = control_loop.ControlLoopWriter("Shooter", [shooter])
if argv[1][-3:] == '.cc':
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 56fd74b..b548365 100644
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -1,4 +1,4 @@
-#include "frc971/control_loops/shooters/shooters.h"
+#include "frc971/control_loops/shooter/shooter.h"
#include <stdio.h>
@@ -8,8 +8,7 @@
#include "aos/common/logging/logging.h"
#include "frc971/constants.h"
-#include "frc971/control_loops/shooters/top_shooter_motor_plant.h"
-#include "frc971/control_loops/shooters/bottom_shooter_motor_plant.h"
+#include "frc971/control_loops/shooter/shooter_motor_plant.h"
namespace frc971 {
namespace control_loops {
@@ -23,11 +22,10 @@
config_data.lower_limit = GetValues().shooter_lower_limit;
config_data.upper_limit = GetValues().shooter_upper_limit;
- config_data.hall_effect_start_position[0] =
- GetValues().shooter_hall_effect_start_position;
+ //config_data.hall_effect_start_position[0] =
+ // GetValues().shooter_hall_effect_start_position;
config_data.zeroing_off_speed = GetValues().shooter_zeroing_off_speed;
config_data.zeroing_speed = GetValues().shooter_zeroing_speed;
-
config_data.max_zeroing_voltage = 5.0;
config_data.deadband_voltage = 0.0;
@@ -37,10 +35,10 @@
// Positive is up, and positive power is up.
void ShooterMotor::RunIteration(
- const ::aos::control_loops::Goal *goal,
+ const ShooterLoop::Goal *goal,
const control_loops::ShooterLoop::Position *position,
- ::aos::control_loops::Output *output,
- ::aos::control_loops::Status * status) {
+ ShooterLoop::Output *output,
+ ShooterLoop::Status * status) {
// Disable the motors now so that all early returns will return with the
// motors disabled.
@@ -54,9 +52,9 @@
if (!position) {
transformed_position_ptr = NULL;
} else {
- transformed_position.position = position->pos;
- transformed_position.hall_effects[0] = position->hall_effect;
- transformed_position.hall_effect_positions[0] = position->calibration;
+ //transformed_position.position = position->pos;
+ //transformed_position.hall_effects[0] = position->hall_effect;
+ //transformed_position.hall_effect_positions[0] = position->calibration;
}
const double voltage = zeroed_joint_.Update(transformed_position_ptr,
@@ -64,9 +62,9 @@
goal->goal, 0.0);
if (position) {
- LOG(DEBUG, "pos: %f hall: %s absolute: %f\n",
- position->pos,
- position->hall_effect ? "true" : "false",
+ LOG(DEBUG, "pos: hall: absolute: %f\n",
+ //position->pos,
+ //position->hall_effect ? "true" : "false",
zeroed_joint_.absolute_position());
}
diff --git a/frc971/control_loops/shooter/shooter.gyp b/frc971/control_loops/shooter/shooter.gyp
index c4e9971..9811918 100644
--- a/frc971/control_loops/shooter/shooter.gyp
+++ b/frc971/control_loops/shooter/shooter.gyp
@@ -23,6 +23,7 @@
'sources': [
'shooter.cc',
'shooter_motor_plant.cc',
+ 'unaugmented_shooter_motor_plant.cc',
],
'dependencies': [
'shooter_loop',
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
new file mode 100644
index 0000000..a6615bf
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter.h
@@ -0,0 +1,63 @@
+#ifndef FRC971_CONTROL_LOOPS_shooter_shooter_H_
+#define FRC971_CONTROL_LOOPS_shooter_shooter_H_
+
+#include <memory>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+#include "frc971/control_loops/shooter/shooter_motor_plant.h"
+#include "frc971/control_loops/shooter/shooter.q.h"
+
+#include "frc971/control_loops/zeroed_joint.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+class ShooterTest_NoWindupPositive_Test;
+class ShooterTest_NoWindupNegative_Test;
+};
+
+class ShooterMotor
+ : public aos::control_loops::ControlLoop<control_loops::ShooterLoop> {
+ public:
+ explicit ShooterMotor(
+ control_loops::ShooterLoop *my_shooter = &control_loops::shooter);
+
+ // True if the goal was moved to avoid goal windup.
+ bool capped_goal() const { return zeroed_joint_.capped_goal(); }
+
+ // True if the shooter is zeroing.
+ bool is_zeroing() const { return zeroed_joint_.is_zeroing(); }
+
+ // True if the shooter is zeroing.
+ bool is_moving_off() const { return zeroed_joint_.is_moving_off(); }
+
+ // True if the state machine is uninitialized.
+ bool is_uninitialized() const { return zeroed_joint_.is_uninitialized(); }
+
+ // True if the state machine is ready.
+ bool is_ready() const { return zeroed_joint_.is_ready(); }
+
+ protected:
+ virtual void RunIteration(
+ const ShooterLoop::Goal *goal,
+ const control_loops::ShooterLoop::Position *position,
+ ShooterLoop::Output *output,
+ ShooterLoop::Status *status);
+
+ private:
+ // Friend the test classes for acces to the internal state.
+ friend class testing::ShooterTest_NoWindupPositive_Test;
+ friend class testing::ShooterTest_NoWindupNegative_Test;
+
+ // The zeroed joint to use.
+ ZeroedJoint<1> zeroed_joint_;
+
+ DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_shooter_shooter_H_
diff --git a/frc971/control_loops/shooter/shooter.q b/frc971/control_loops/shooter/shooter.q
index 860162e..7a487bc 100644
--- a/frc971/control_loops/shooter/shooter.q
+++ b/frc971/control_loops/shooter/shooter.q
@@ -1,13 +1,21 @@
package frc971.control_loops;
-import "aos/common/control_loop/control_looops.q";
+import "aos/common/control_loop/control_loops.q";
queue_group ShooterLoop {
implements aos.control_loops.ControlLoop;
+ message Output {
+ // The energy to load to in joules.
+ double voltage;
+ // Shoots as soon as this is true.
+ bool latched;
+ bool locked; //Disc brake locked
+ };
message Goal {
// The energy to load to in joules.
double energy;
+ double goal;
// Shoots as soon as this is true.
bool shoot;
};
@@ -23,12 +31,13 @@
// Whether the plunger is in and out of the way of grabbing a ball.
bool cocked;
// How many times we've shot.
- int shots;
+ int32_t shots;
+ bool done;
};
queue Goal goal;
queue Position position;
- queue aos.control_loops.Output output;
+ queue Output output;
queue Status status;
};
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
new file mode 100644
index 0000000..a111917
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -0,0 +1,161 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/shooter/shooter.q.h"
+#include "frc971/control_loops/shooter/shooter.h"
+#include "frc971/control_loops/shooter/unaugmented_shooter_motor_plant.h"
+#include "frc971/constants.h"
+
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+// Class which simulates the shooter and sends out queue messages containing the
+// position.
+class ShooterMotorSimulation {
+ public:
+ // Constructs a motor simulation. initial_position is the inital angle of the
+ // shooter, which will be treated as 0 by the encoder.
+ ShooterMotorSimulation(double initial_position)
+ : shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawShooterPlant())),
+ my_shooter_loop_(".frc971.control_loops.shooter",
+ 0x1a7b7094, ".frc971.control_loops.shooter.goal",
+ ".frc971.control_loops.shooter.position",
+ ".frc971.control_loops.shooter.output",
+ ".frc971.control_loops.shooter.status") {
+
+ printf("test");
+ Reinitialize(initial_position);
+ }
+
+ // Resets the plant so that it starts at initial_position.
+ void Reinitialize(double initial_position) {
+ if(initial_position)
+ return;
+ else return;
+ }
+
+ // Returns the absolute angle of the shooter.
+ double GetAbsolutePosition() const {
+ return 0.0;
+ }
+
+ // Returns the adjusted angle of the shooter.
+ double GetPosition() const {
+ return 0.0;
+ }
+
+ // Sends out the position queue messages.
+ void SendPositionMessage() {
+ }
+
+ // Simulates the shooter moving for one timestep.
+ void Simulate() {
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_;
+ private:
+ ShooterLoop my_shooter_loop_;
+ double initial_position_;
+ double last_position_;
+ double calibration_value_;
+ double last_voltage_;
+};
+
+class ShooterTest : public ::testing::Test {
+ protected:
+ ::aos::common::testing::GlobalCoreInstance my_core;
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointer to shared memory that
+ // is no longer valid.
+ ShooterLoop my_shooter_loop_;
+
+ // Create a loop and simulation plant.
+ ShooterMotor shooter_motor_;
+ ShooterMotorSimulation shooter_motor_plant_;
+
+ ShooterTest() : my_shooter_loop_(".frc971.control_loops.shooter",
+ 0x1a7b7094, ".frc971.control_loops.shooter.goal",
+ ".frc971.control_loops.shooter.position",
+ ".frc971.control_loops.shooter.output",
+ ".frc971.control_loops.shooter.status"),
+ shooter_motor_(&my_shooter_loop_),
+ shooter_motor_plant_(0.5) {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ SendDSPacket(true);
+ }
+
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ void VerifyNearGoal() {
+ my_shooter_loop_.goal.FetchLatest();
+ my_shooter_loop_.position.FetchLatest();
+ EXPECT_NEAR(my_shooter_loop_.goal->goal,
+ shooter_motor_plant_.GetAbsolutePosition(),
+ 1e-4);
+ }
+
+ virtual ~ShooterTest() {
+ ::aos::robot_state.Clear();
+ }
+};
+
+// Tests that the shooter zeros correctly and goes to a position.
+//TEST_F(ShooterTest, ZerosCorrectly) {
+// my_shooter.goal.MakeWithBuilder().goal(0.1).Send();
+// for (int i = 0; i < 400; ++i) {
+// shooter_motor_plant_.SendPositionMessage();
+// shooter_motor_.Iterate();
+// shooter_motor_plant_.Simulate();
+// SendDSPacket(true);
+// }
+// VerifyNearGoal();
+//}
+
+// Tests that the shooter zeros correctly starting on the hall effect sensor.
+TEST_F(ShooterTest, ZerosStartingOn) {
+ printf("test");
+ EXPECT_TRUE(true);
+}
+
+//// Tests that missing positions are correctly handled.
+//TEST_F(ShooterTest, HandleMissingPosition) {
+//}
+//
+//// Tests that losing the encoder for a second triggers a re-zero.
+//TEST_F(ShooterTest, RezeroWithMissingPos) {
+//}
+//
+//// Tests that disabling while zeroing sends the state machine into the
+//// uninitialized state.
+//TEST_F(ShooterTest, DisableGoesUninitialized) {
+//}
+//
+//// Tests that the shooter can't get too far away from the zeroing position if the
+//// zeroing position is saturating the goal.
+//TEST_F(ShooterTest, NoWindupNegative) {
+//}
+//
+//// Tests that the shooter can't get too far away from the zeroing position if the
+//// zeroing position is saturating the goal.
+//TEST_F(ShooterTest, NoWindupPositive) {
+//}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/shooter/shooter_main.cc b/frc971/control_loops/shooter/shooter_main.cc
index cb6fc8b..e4e25ad 100644
--- a/frc971/control_loops/shooter/shooter_main.cc
+++ b/frc971/control_loops/shooter/shooter_main.cc
@@ -4,7 +4,7 @@
int main() {
::aos::Init();
- frc971::control_loops::ShooterLoop shooter;
+ frc971::control_loops::ShooterMotor shooter;
shooter.Run();
::aos::Cleanup();
return 0;
diff --git a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc
new file mode 100644
index 0000000..d395200
--- /dev/null
+++ b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/shooter/unaugmented_shooter_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawShooterPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00988697090637, 0.0, 0.977479674375;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.000120553991591, 0.0240196135246;
+ 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> MakeRawShooterController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.87747967438, 87.0117404538;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 343.469306513, 26.4814035344;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeRawShooterPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeRawShooterPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawShooterPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeRawShooterLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeRawShooterController());
+ return StateFeedbackLoop<2, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.h b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.h
new file mode 100644
index 0000000..4deea48
--- /dev/null
+++ b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_SHOOTER_UNAUGMENTED_SHOOTER_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_SHOOTER_UNAUGMENTED_SHOOTER_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawShooterPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeRawShooterController();
+
+StateFeedbackPlant<2, 1, 1> MakeRawShooterPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeRawShooterLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_SHOOTER_UNAUGMENTED_SHOOTER_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/update_shooter.sh b/frc971/control_loops/update_shooter.sh
index 8a18289..a9c5807 100755
--- a/frc971/control_loops/update_shooter.sh
+++ b/frc971/control_loops/update_shooter.sh
@@ -5,4 +5,6 @@
cd $(dirname $0)
./python/shooter.py shooter/shooter_motor_plant.h \
- shooter/shooter_motor_plant.cc
+ shooter/shooter_motor_plant.cc \
+ shooter/unaugmented_shooter_motor_plant.h \
+ shooter/unaugmented_shooter_motor_plant.cc
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index d841672..0b59c0b 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -7,6 +7,8 @@
'<(AOS)/build/aos_all.gyp:Prime',
'../control_loops/drivetrain/drivetrain.gyp:drivetrain',
'../control_loops/drivetrain/drivetrain.gyp:drivetrain_lib_test',
+ '../control_loops/shooter/shooter.gyp:shooter',
+ '../control_loops/shooter/shooter.gyp:shooter_lib_test',
'../autonomous/autonomous.gyp:auto',
'../input/input.gyp:joystick_reader',
'../output/output.gyp:motor_writer',