Renamed everything to claw.
- Renamed all the wrists calls to claw.
- Added a top and bottom wrist controller.
diff --git a/frc971/constants.cc b/frc971/constants.cc
index ad5e191..9893f90 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -53,6 +53,11 @@
true,
control_loops::MakeVClutchDrivetrainLoop,
control_loops::MakeClutchDrivetrainLoop,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
};
break;
case kPracticeTeamNumber:
@@ -65,6 +70,11 @@
false,
control_loops::MakeVDogDrivetrainLoop,
control_loops::MakeDogDrivetrainLoop,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
};
break;
default:
diff --git a/frc971/constants.h b/frc971/constants.h
index 2d12bfa..9691bb2 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -39,6 +39,12 @@
::std::function<StateFeedbackLoop<2, 2, 2>()> make_v_drivetrain_loop;
::std::function<StateFeedbackLoop<4, 2, 2>()> make_drivetrain_loop;
+
+ double claw_lower_limit;
+ double claw_upper_limit;
+ double claw_hall_effect_start_angle;
+ double claw_zeroing_off_speed;
+ double claw_zeroing_speed;
};
// Creates (once) a Values instance and returns a reference to it.
diff --git a/frc971/control_loops/wrists/top_wrist_motor_plant.cc b/frc971/control_loops/claw/bottom_claw_motor_plant.cc
similarity index 65%
copy from frc971/control_loops/wrists/top_wrist_motor_plant.cc
copy to frc971/control_loops/claw/bottom_claw_motor_plant.cc
index 132ef4f..a0eb131 100644
--- a/frc971/control_loops/wrists/top_wrist_motor_plant.cc
+++ b/frc971/control_loops/claw/bottom_claw_motor_plant.cc
@@ -1,4 +1,4 @@
-#include "frc971/control_loops/wrists/top_wrist_motor_plant.h"
+#include "frc971/control_loops/claw/bottom_claw_motor_plant.h"
#include <vector>
@@ -7,7 +7,7 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<3, 1, 1> MakeWristPlantCoefficients() {
+StateFeedbackPlantCoefficients<3, 1, 1> MakeBottomClawPlantCoefficients() {
Eigen::Matrix<double, 3, 3> A;
A << 1.0, 0.00904786878843, 0.000326582411818, 0.0, 0.815818233346, 0.0631746179893, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 3, 1> B;
@@ -23,23 +23,23 @@
return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
}
-StateFeedbackController<3, 1, 1> MakeWristController() {
+StateFeedbackController<3, 1, 1> MakeBottomClawController() {
Eigen::Matrix<double, 3, 1> L;
L << 1.81581823335, 78.6334534274, 142.868137351;
Eigen::Matrix<double, 1, 3> K;
K << 92.6004807973, 4.38063492858, 1.11581823335;
- return StateFeedbackController<3, 1, 1>(L, K, MakeWristPlantCoefficients());
+ return StateFeedbackController<3, 1, 1>(L, K, MakeBottomClawPlantCoefficients());
}
-StateFeedbackPlant<3, 1, 1> MakeWristPlant() {
+StateFeedbackPlant<3, 1, 1> MakeBottomClawPlant() {
::std::vector<StateFeedbackPlantCoefficients<3, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<3, 1, 1>(MakeWristPlantCoefficients());
+ plants[0] = new StateFeedbackPlantCoefficients<3, 1, 1>(MakeBottomClawPlantCoefficients());
return StateFeedbackPlant<3, 1, 1>(plants);
}
-StateFeedbackLoop<3, 1, 1> MakeWristLoop() {
+StateFeedbackLoop<3, 1, 1> MakeBottomClawLoop() {
::std::vector<StateFeedbackController<3, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<3, 1, 1>(MakeWristController());
+ controllers[0] = new StateFeedbackController<3, 1, 1>(MakeBottomClawController());
return StateFeedbackLoop<3, 1, 1>(controllers);
}
diff --git a/frc971/control_loops/claw/bottom_claw_motor_plant.h b/frc971/control_loops/claw/bottom_claw_motor_plant.h
new file mode 100644
index 0000000..fc905ca
--- /dev/null
+++ b/frc971/control_loops/claw/bottom_claw_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_CLAW_BOTTOM_CLAW_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_CLAW_BOTTOM_CLAW_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeBottomClawPlantCoefficients();
+
+StateFeedbackController<3, 1, 1> MakeBottomClawController();
+
+StateFeedbackPlant<3, 1, 1> MakeBottomClawPlant();
+
+StateFeedbackLoop<3, 1, 1> MakeBottomClawLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_CLAW_BOTTOM_CLAW_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
new file mode 100644
index 0000000..55cbcc5
--- /dev/null
+++ b/frc971/control_loops/claw/claw.cc
@@ -0,0 +1,113 @@
+#include "frc971/control_loops/claw/claw.h"
+
+#include <stdio.h>
+
+#include <algorithm>
+
+#include "aos/common/control_loop/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/claw/top_claw_motor_plant.h"
+#include "frc971/control_loops/claw/bottom_claw_motor_plant.h"
+
+// Zeroing plan.
+// There are 2 types of zeros. Enabled and disabled ones.
+// Disabled ones are only valid during auto mode, and can be used to speed up
+// the enabled zero process. We need to re-zero during teleop in case the auto
+// zero was poor and causes us to miss all our shots.
+//
+// We need to be able to zero manually while disabled by moving the joint over
+// the zeros.
+// Zero on the down edge when disabled (gravity in the direction of motion)
+//
+// When enabled, zero on the up edge (gravity opposing the direction of motion)
+// The enabled sequence needs to work as follows. We can crash the claw if we
+// bring them too close to each other or too far from each other. The only safe
+// thing to do is to move them in unison.
+//
+// Start by moving them both towards the front of the bot to either find either
+// the middle hall effect on either jaw, or the front hall effect on the bottom
+// jaw. Any edge that isn't the desired edge will provide an approximate edge
+// location that can be used for the fine tuning step.
+// Once an edge is found on the front claw, move back the other way with both
+// claws until an edge is found for the other claw.
+// Now that we have an approximate zero, we can robustify the limits to keep
+// both claws safe. Then, we can move both claws to a position that is the
+// correct side of the zero and go zero.
+
+// Valid region plan.
+// Difference between the arms has a range, and the values of each arm has a range.
+// If a claw runs up against a static limit, don't let the goal change outside
+// the limit.
+// If a claw runs up against a movable limit, move both claws outwards to get
+// out of the condition.
+
+namespace frc971 {
+namespace control_loops {
+
+ClawMotor::ClawMotor(control_loops::ClawLoop *my_claw)
+ : aos::control_loops::ControlLoop<control_loops::ClawLoop>(my_claw),
+ zeroed_joint_(MakeTopClawLoop()) {
+ {
+ using ::frc971::constants::GetValues;
+ ZeroedJoint<1>::ConfigurationData config_data;
+
+ config_data.lower_limit = GetValues().claw_lower_limit;
+ config_data.upper_limit = GetValues().claw_upper_limit;
+ config_data.hall_effect_start_angle[0] =
+ GetValues().claw_hall_effect_start_angle;
+ config_data.zeroing_off_speed = GetValues().claw_zeroing_off_speed;
+ config_data.zeroing_speed = GetValues().claw_zeroing_speed;
+
+ config_data.max_zeroing_voltage = 5.0;
+ config_data.deadband_voltage = 0.0;
+
+ zeroed_joint_.set_config_data(config_data);
+ }
+}
+
+// Positive angle is up, and positive power is up.
+void ClawMotor::RunIteration(const control_loops::ClawLoop::Goal *goal,
+ const control_loops::ClawLoop::Position *position,
+ control_loops::ClawLoop::Output *output,
+ ::aos::control_loops::Status *status) {
+
+ // Disable the motors now so that all early returns will return with the
+ // motors disabled.
+ if (output) {
+ output->top_claw_voltage = 0;
+ output->bottom_claw_voltage = 0;
+ output->intake_voltage = 0;
+ }
+
+ ZeroedJoint<1>::PositionData transformed_position;
+ ZeroedJoint<1>::PositionData *transformed_position_ptr =
+ &transformed_position;
+ if (!position) {
+ transformed_position_ptr = NULL;
+ } else {
+ transformed_position.position = position->top_position;
+ transformed_position.hall_effects[0] = position->top_calibration_hall_effect;
+ transformed_position.hall_effect_positions[0] = position->top_posedge_value;
+ }
+
+ const double voltage =
+ zeroed_joint_.Update(transformed_position_ptr, output != NULL,
+ goal->bottom_angle + goal->seperation_angle, 0.0);
+
+ if (position) {
+ LOG(DEBUG, "pos: %f hall: %s absolute: %f\n", position->top_position,
+ position->top_calibration_hall_effect ? "true" : "false",
+ zeroed_joint_.absolute_position());
+ }
+
+ if (output) {
+ output->top_claw_voltage = voltage;
+ }
+ status->done = ::std::abs(zeroed_joint_.absolute_position() -
+ goal->bottom_angle - goal->seperation_angle) < 0.004;
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/wrists/wrists.gyp b/frc971/control_loops/claw/claw.gyp
similarity index 69%
rename from frc971/control_loops/wrists/wrists.gyp
rename to frc971/control_loops/claw/claw.gyp
index f455e2e..c541dad 100644
--- a/frc971/control_loops/wrists/wrists.gyp
+++ b/frc971/control_loops/claw/claw.gyp
@@ -1,11 +1,11 @@
{
'targets': [
{
- 'target_name': 'wrist_loops',
+ 'target_name': 'claw_loops',
'type': 'static_library',
- 'sources': ['wrists.q'],
+ 'sources': ['claw.q'],
'variables': {
- 'header_path': 'frc971/control_loops/wrists',
+ 'header_path': 'frc971/control_loops/claw',
},
'dependencies': [
'<(AOS)/common/common.gyp:control_loop_queues',
@@ -18,48 +18,50 @@
'includes': ['../../../aos/build/queues.gypi'],
},
{
- 'target_name': 'wrists_lib',
+ 'target_name': 'claw_lib',
'type': 'static_library',
'sources': [
- 'wrists.cc',
- 'top_wrist_motor_plant.cc',
- 'top_wrist_motor_plant.cc',
+ 'claw.cc',
+ 'top_claw_motor_plant.cc',
+ 'bottom_claw_motor_plant.cc',
+ 'unaugmented_top_claw_motor_plant.cc',
+ 'unaugmented_bottom_claw_motor_plant.cc',
],
'dependencies': [
- 'wrist_loops',
+ 'claw_loops',
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
],
'export_dependent_settings': [
- 'wrist_loops',
+ 'claw_loops',
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
],
},
{
- 'target_name': 'wrists_lib_test',
+ 'target_name': 'claw_lib_test',
'type': 'executable',
'sources': [
- 'wrists_lib_test.cc',
+ 'claw_lib_test.cc',
],
'dependencies': [
'<(EXTERNALS):gtest',
- 'wrist_loops',
- 'wrists_lib',
+ 'claw_loops',
+ 'claw_lib',
'<(AOS)/common/common.gyp:queue_testutils',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
],
},
{
- 'target_name': 'wrists',
+ 'target_name': 'claw',
'type': 'executable',
'sources': [
- 'wrists_main.cc',
+ 'claw_main.cc',
],
'dependencies': [
'<(AOS)/linux_code/linux_code.gyp:init',
- 'wrists_lib',
+ 'claw_lib',
],
},
],
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
new file mode 100644
index 0000000..e057c3e
--- /dev/null
+++ b/frc971/control_loops/claw/claw.h
@@ -0,0 +1,63 @@
+#ifndef FRC971_CONTROL_LOOPS_CLAW_CLAW_H_
+#define FRC971_CONTROL_LOOPS_CLAW_CLAW_H_
+
+#include <memory>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/control_loops/claw/top_claw_motor_plant.h"
+#include "frc971/control_loops/claw/bottom_claw_motor_plant.h"
+
+#include "frc971/control_loops/zeroed_joint.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+class ClawTest_NoWindupPositive_Test;
+class ClawTest_NoWindupNegative_Test;
+};
+
+class ClawMotor
+ : public aos::control_loops::ControlLoop<control_loops::ClawLoop> {
+ public:
+ explicit ClawMotor(
+ control_loops::ClawLoop *my_claw = &control_loops::claw);
+
+ // True if the goal was moved to avoid goal windup.
+ bool capped_goal() const { return zeroed_joint_.capped_goal(); }
+
+ // True if the claw is zeroing.
+ bool is_zeroing() const { return zeroed_joint_.is_zeroing(); }
+
+ // True if the claw 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 control_loops::ClawLoop::Goal *goal,
+ const control_loops::ClawLoop::Position *position,
+ control_loops::ClawLoop::Output *output,
+ ::aos::control_loops::Status *status);
+
+ private:
+ // Friend the test classes for acces to the internal state.
+ friend class testing::ClawTest_NoWindupPositive_Test;
+ friend class testing::ClawTest_NoWindupNegative_Test;
+
+ // The zeroed joint to use.
+ ZeroedJoint<1> zeroed_joint_;
+
+ DISALLOW_COPY_AND_ASSIGN(ClawMotor);
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_CLAW_CLAW_H_
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
new file mode 100644
index 0000000..8852fd3
--- /dev/null
+++ b/frc971/control_loops/claw/claw.q
@@ -0,0 +1,57 @@
+package frc971.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+// All angles here are 0 horizontal, positive up.
+queue_group ClawLoop {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ // The angle of the bottom wrist.
+ double bottom_angle;
+ // How much higher the top wrist is.
+ double seperation_angle;
+ bool intake;
+ };
+ message Position {
+ double top_position;
+
+ bool top_front_hall_effect;
+ int32_t top_front_hall_effect_posedge_count;
+ int32_t top_front_hall_effect_negedge_count;
+ bool top_calibration_hall_effect;
+ int32_t top_calibration_hall_effect_posedge_count;
+ int32_t top_calibration_hall_effect_negedge_count;
+ bool top_back_hall_effect;
+ int32_t top_back_hall_effect_posedge_count;
+ int32_t top_back_hall_effect_negedge_count;
+ double top_posedge_value;
+ double top_negedge_value;
+
+ double bottom_position;
+ bool bottom_front_hall_effect;
+ int32_t bottom_front_hall_effect_posedge_count;
+ int32_t bottom_front_hall_effect_negedge_count;
+ bool bottom_calibration_hall_effect;
+ int32_t bottom_calibration_hall_effect_posedge_count;
+ int32_t bottom_calibration_hall_effect_negedge_count;
+ bool bottom_back_hall_effect;
+ int32_t bottom_back_hall_effect_posedge_count;
+ int32_t bottom_back_hall_effect_negedge_count;
+ double bottom_posedge_value;
+ double bottom_negedge_value;
+ };
+
+ message Output {
+ double intake_voltage;
+ double top_claw_voltage;
+ double bottom_claw_voltage;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue aos.control_loops.Status status;
+};
+
+queue_group ClawLoop claw;
diff --git a/frc971/control_loops/wrists/wrists_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
similarity index 89%
rename from frc971/control_loops/wrists/wrists_lib_test.cc
rename to frc971/control_loops/claw/claw_lib_test.cc
index 996036e..e881ef2 100644
--- a/frc971/control_loops/wrists/wrists_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -5,8 +5,8 @@
#include "gtest/gtest.h"
#include "aos/common/queue.h"
#include "aos/common/queue_testutils.h"
-#include "frc971/control_loops/wrists/wrists.q.h"
-#include "frc971/control_loops/wrists/wrists.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/control_loops/claw/claw.h"
#include "frc971/constants.h"
@@ -18,17 +18,17 @@
// Class which simulates the wrist and sends out queue messages containing the
// position.
-class WristMotorSimulation {
+class ClawMotorSimulation {
public:
// Constructs a motor simulation. initial_position is the inital angle of the
// wrist, which will be treated as 0 by the encoder.
- WristMotorSimulation(double initial_position)
- : wrist_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawWristPlant())),
- my_wrist_loop_(".frc971.control_loops.wrists",
- 0x1a7b7094, ".frc971.control_loops.wrists.goal",
- ".frc971.control_loops.wrists.position",
- ".frc971.control_loops.wrists.output",
- ".frc971.control_loops.wrists.status") {
+ ClawMotorSimulation(double initial_position)
+ : wrist_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawClawPlant())),
+ my_wrist_loop_(".frc971.control_loops.claw",
+ 0x1a7b7094, ".frc971.control_loops.claw.goal",
+ ".frc971.control_loops.claw.position",
+ ".frc971.control_loops.claw.output",
+ ".frc971.control_loops.claw.status") {
Reinitialize(initial_position);
}
@@ -57,7 +57,7 @@
void SendPositionMessage() {
const double angle = GetPosition();
- ::aos::ScopedMessagePtr<control_loops::WristLoop::Position> position =
+ ::aos::ScopedMessagePtr<control_loops::ClawLoop::Position> position =
my_wrist_loop_.position.MakeMessage();
position->pos = angle;
@@ -103,27 +103,27 @@
::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> wrist_plant_;
private:
- WristLoop my_wrist_loop_;
+ ClawLoop my_wrist_loop_;
double initial_position_;
double last_position_;
double calibration_value_;
double last_voltage_;
};
-class WristTest : public ::testing::Test {
+class ClawTest : 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.
- WristLoop my_wrist_loop_;
+ ClawLoop my_wrist_loop_;
// Create a loop and simulation plant.
- WristMotor wrist_motor_;
- WristMotorSimulation wrist_motor_plant_;
+ ClawMotor wrist_motor_;
+ ClawMotorSimulation wrist_motor_plant_;
- WristTest() : my_wrist_loop_(".frc971.control_loops.wrist",
+ ClawTest() : my_wrist_loop_(".frc971.control_loops.wrist",
0x1a7b7094, ".frc971.control_loops.wrist.goal",
".frc971.control_loops.wrist.position",
".frc971.control_loops.wrist.output",
@@ -151,13 +151,13 @@
1e-4);
}
- virtual ~WristTest() {
+ virtual ~ClawTest() {
::aos::robot_state.Clear();
}
};
// Tests that the wrist zeros correctly and goes to a position.
-TEST_F(WristTest, ZerosCorrectly) {
+TEST_F(ClawTest, ZerosCorrectly) {
my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
for (int i = 0; i < 400; ++i) {
wrist_motor_plant_.SendPositionMessage();
@@ -169,7 +169,7 @@
}
// Tests that the wrist zeros correctly starting on the hall effect sensor.
-TEST_F(WristTest, ZerosStartingOn) {
+TEST_F(ClawTest, ZerosStartingOn) {
wrist_motor_plant_.Reinitialize(90 * M_PI / 180.0);
my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
@@ -183,7 +183,7 @@
}
// Tests that missing positions are correctly handled.
-TEST_F(WristTest, HandleMissingPosition) {
+TEST_F(ClawTest, HandleMissingPosition) {
my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
for (int i = 0; i < 400; ++i) {
if (i % 23) {
@@ -197,7 +197,7 @@
}
// Tests that loosing the encoder for a second triggers a re-zero.
-TEST_F(WristTest, RezeroWithMissingPos) {
+TEST_F(ClawTest, RezeroWithMissingPos) {
my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
for (int i = 0; i < 800; ++i) {
// After 3 seconds, simulate the encoder going missing.
@@ -225,7 +225,7 @@
// Tests that disabling while zeroing sends the state machine into the
// uninitialized state.
-TEST_F(WristTest, DisableGoesUninitialized) {
+TEST_F(ClawTest, DisableGoesUninitialized) {
my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
for (int i = 0; i < 800; ++i) {
wrist_motor_plant_.SendPositionMessage();
@@ -256,7 +256,7 @@
// Tests that the wrist can't get too far away from the zeroing position if the
// zeroing position is saturating the goal.
-TEST_F(WristTest, NoWindupNegative) {
+TEST_F(ClawTest, NoWindupNegative) {
int capped_count = 0;
double saved_zeroing_position = 0;
my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
@@ -293,7 +293,7 @@
// Tests that the wrist can't get too far away from the zeroing position if the
// zeroing position is saturating the goal.
-TEST_F(WristTest, NoWindupPositive) {
+TEST_F(ClawTest, NoWindupPositive) {
int capped_count = 0;
double saved_zeroing_position = 0;
my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
diff --git a/frc971/control_loops/claw/claw_main.cc b/frc971/control_loops/claw/claw_main.cc
new file mode 100644
index 0000000..3b7c181
--- /dev/null
+++ b/frc971/control_loops/claw/claw_main.cc
@@ -0,0 +1,11 @@
+#include "frc971/control_loops/claw/claw.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+ ::aos::Init();
+ frc971::control_loops::ClawMotor claw;
+ claw.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/control_loops/wrists/top_wrist_motor_plant.cc b/frc971/control_loops/claw/top_claw_motor_plant.cc
similarity index 66%
rename from frc971/control_loops/wrists/top_wrist_motor_plant.cc
rename to frc971/control_loops/claw/top_claw_motor_plant.cc
index 132ef4f..113ff77 100644
--- a/frc971/control_loops/wrists/top_wrist_motor_plant.cc
+++ b/frc971/control_loops/claw/top_claw_motor_plant.cc
@@ -1,4 +1,4 @@
-#include "frc971/control_loops/wrists/top_wrist_motor_plant.h"
+#include "frc971/control_loops/claw/top_claw_motor_plant.h"
#include <vector>
@@ -7,7 +7,7 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<3, 1, 1> MakeWristPlantCoefficients() {
+StateFeedbackPlantCoefficients<3, 1, 1> MakeTopClawPlantCoefficients() {
Eigen::Matrix<double, 3, 3> A;
A << 1.0, 0.00904786878843, 0.000326582411818, 0.0, 0.815818233346, 0.0631746179893, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 3, 1> B;
@@ -23,23 +23,23 @@
return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
}
-StateFeedbackController<3, 1, 1> MakeWristController() {
+StateFeedbackController<3, 1, 1> MakeTopClawController() {
Eigen::Matrix<double, 3, 1> L;
L << 1.81581823335, 78.6334534274, 142.868137351;
Eigen::Matrix<double, 1, 3> K;
K << 92.6004807973, 4.38063492858, 1.11581823335;
- return StateFeedbackController<3, 1, 1>(L, K, MakeWristPlantCoefficients());
+ return StateFeedbackController<3, 1, 1>(L, K, MakeTopClawPlantCoefficients());
}
-StateFeedbackPlant<3, 1, 1> MakeWristPlant() {
+StateFeedbackPlant<3, 1, 1> MakeTopClawPlant() {
::std::vector<StateFeedbackPlantCoefficients<3, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<3, 1, 1>(MakeWristPlantCoefficients());
+ plants[0] = new StateFeedbackPlantCoefficients<3, 1, 1>(MakeTopClawPlantCoefficients());
return StateFeedbackPlant<3, 1, 1>(plants);
}
-StateFeedbackLoop<3, 1, 1> MakeWristLoop() {
+StateFeedbackLoop<3, 1, 1> MakeTopClawLoop() {
::std::vector<StateFeedbackController<3, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<3, 1, 1>(MakeWristController());
+ controllers[0] = new StateFeedbackController<3, 1, 1>(MakeTopClawController());
return StateFeedbackLoop<3, 1, 1>(controllers);
}
diff --git a/frc971/control_loops/claw/top_claw_motor_plant.h b/frc971/control_loops/claw/top_claw_motor_plant.h
new file mode 100644
index 0000000..c74c976
--- /dev/null
+++ b/frc971/control_loops/claw/top_claw_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_CLAW_TOP_CLAW_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_CLAW_TOP_CLAW_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeTopClawPlantCoefficients();
+
+StateFeedbackController<3, 1, 1> MakeTopClawController();
+
+StateFeedbackPlant<3, 1, 1> MakeTopClawPlant();
+
+StateFeedbackLoop<3, 1, 1> MakeTopClawLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_CLAW_TOP_CLAW_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/wrists/bottom_wrist_motor_plant.cc b/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.cc
similarity index 66%
copy from frc971/control_loops/wrists/bottom_wrist_motor_plant.cc
copy to frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.cc
index f08793b..cda15c4 100644
--- a/frc971/control_loops/wrists/bottom_wrist_motor_plant.cc
+++ b/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.cc
@@ -1,4 +1,4 @@
-#include "frc971/control_loops/wrists/bottom_wrist_motor_plant.h"
+#include "frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.h"
#include <vector>
@@ -7,7 +7,7 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<2, 1, 1> MakeRawWristPlantCoefficients() {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawBottomClawPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
A << 1.0, 0.00904786878843, 0.0, 0.815818233346;
Eigen::Matrix<double, 2, 1> B;
@@ -23,23 +23,23 @@
return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
}
-StateFeedbackController<2, 1, 1> MakeRawWristController() {
+StateFeedbackController<2, 1, 1> MakeRawBottomClawController() {
Eigen::Matrix<double, 2, 1> L;
L << 1.71581823335, 64.8264890043;
Eigen::Matrix<double, 1, 2> K;
K << 130.590421637, 7.48987035533;
- return StateFeedbackController<2, 1, 1>(L, K, MakeRawWristPlantCoefficients());
+ return StateFeedbackController<2, 1, 1>(L, K, MakeRawBottomClawPlantCoefficients());
}
-StateFeedbackPlant<2, 1, 1> MakeRawWristPlant() {
+StateFeedbackPlant<2, 1, 1> MakeRawBottomClawPlant() {
::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawWristPlantCoefficients());
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawBottomClawPlantCoefficients());
return StateFeedbackPlant<2, 1, 1>(plants);
}
-StateFeedbackLoop<2, 1, 1> MakeRawWristLoop() {
+StateFeedbackLoop<2, 1, 1> MakeRawBottomClawLoop() {
::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<2, 1, 1>(MakeRawWristController());
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeRawBottomClawController());
return StateFeedbackLoop<2, 1, 1>(controllers);
}
diff --git a/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.h b/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.h
new file mode 100644
index 0000000..8f59925
--- /dev/null
+++ b/frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_BOTTOM_CLAW_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_BOTTOM_CLAW_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawBottomClawPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeRawBottomClawController();
+
+StateFeedbackPlant<2, 1, 1> MakeRawBottomClawPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeRawBottomClawLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_BOTTOM_CLAW_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/wrists/bottom_wrist_motor_plant.cc b/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.cc
similarity index 67%
rename from frc971/control_loops/wrists/bottom_wrist_motor_plant.cc
rename to frc971/control_loops/claw/unaugmented_top_claw_motor_plant.cc
index f08793b..8ab4bbf 100644
--- a/frc971/control_loops/wrists/bottom_wrist_motor_plant.cc
+++ b/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.cc
@@ -1,4 +1,4 @@
-#include "frc971/control_loops/wrists/bottom_wrist_motor_plant.h"
+#include "frc971/control_loops/claw/unaugmented_top_claw_motor_plant.h"
#include <vector>
@@ -7,7 +7,7 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<2, 1, 1> MakeRawWristPlantCoefficients() {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawTopClawPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
A << 1.0, 0.00904786878843, 0.0, 0.815818233346;
Eigen::Matrix<double, 2, 1> B;
@@ -23,23 +23,23 @@
return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
}
-StateFeedbackController<2, 1, 1> MakeRawWristController() {
+StateFeedbackController<2, 1, 1> MakeRawTopClawController() {
Eigen::Matrix<double, 2, 1> L;
L << 1.71581823335, 64.8264890043;
Eigen::Matrix<double, 1, 2> K;
K << 130.590421637, 7.48987035533;
- return StateFeedbackController<2, 1, 1>(L, K, MakeRawWristPlantCoefficients());
+ return StateFeedbackController<2, 1, 1>(L, K, MakeRawTopClawPlantCoefficients());
}
-StateFeedbackPlant<2, 1, 1> MakeRawWristPlant() {
+StateFeedbackPlant<2, 1, 1> MakeRawTopClawPlant() {
::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawWristPlantCoefficients());
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawTopClawPlantCoefficients());
return StateFeedbackPlant<2, 1, 1>(plants);
}
-StateFeedbackLoop<2, 1, 1> MakeRawWristLoop() {
+StateFeedbackLoop<2, 1, 1> MakeRawTopClawLoop() {
::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<2, 1, 1>(MakeRawWristController());
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeRawTopClawController());
return StateFeedbackLoop<2, 1, 1>(controllers);
}
diff --git a/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.h b/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.h
new file mode 100644
index 0000000..c87d3ca
--- /dev/null
+++ b/frc971/control_loops/claw/unaugmented_top_claw_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_TOP_CLAW_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_TOP_CLAW_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawTopClawPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeRawTopClawController();
+
+StateFeedbackPlant<2, 1, 1> MakeRawTopClawPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeRawTopClawLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_CLAW_UNAUGMENTED_TOP_CLAW_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index bcd62f9..7e6159f 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -13,8 +13,8 @@
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/drivetrain/polydrivetrain_cim_plant.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/GyroAngle.q.h"
-#include "frc971/queues/Piston.q.h"
+#include "frc971/queues/gyro_angle.q.h"
+#include "frc971/queues/piston.q.h"
#include "frc971/constants.h"
using frc971::sensors::gyro;
@@ -569,155 +569,6 @@
constexpr double PolyDrivetrain::Kt;
-
-class DrivetrainMotorsOL {
- public:
- DrivetrainMotorsOL() {
- _old_wheel = 0.0;
- wheel_ = 0.0;
- throttle_ = 0.0;
- quickturn_ = false;
- highgear_ = true;
- _neg_inertia_accumulator = 0.0;
- _left_pwm = 0.0;
- _right_pwm = 0.0;
- }
- void SetGoal(double wheel, double throttle, bool quickturn, bool highgear) {
- wheel_ = wheel;
- throttle_ = throttle;
- quickturn_ = quickturn;
- highgear_ = highgear;
- _left_pwm = 0.0;
- _right_pwm = 0.0;
- }
- void Update() {
- double overPower;
- float sensitivity = 1.7;
- float angular_power;
- float linear_power;
- double wheel;
-
- double neg_inertia = wheel_ - _old_wheel;
- _old_wheel = wheel_;
-
- double wheelNonLinearity;
- if (highgear_) {
- wheelNonLinearity = 0.1; // used to be csvReader->TURN_NONLIN_HIGH
- // Apply a sin function that's scaled to make it feel better.
- const double angular_range = M_PI / 2.0 * wheelNonLinearity;
- wheel = sin(angular_range * wheel_) / sin(angular_range);
- wheel = sin(angular_range * wheel) / sin(angular_range);
- } else {
- wheelNonLinearity = 0.2; // used to be csvReader->TURN_NONLIN_LOW
- // Apply a sin function that's scaled to make it feel better.
- const double angular_range = M_PI / 2.0 * wheelNonLinearity;
- wheel = sin(angular_range * wheel_) / sin(angular_range);
- wheel = sin(angular_range * wheel) / sin(angular_range);
- wheel = sin(angular_range * wheel) / sin(angular_range);
- }
-
- static const double kThrottleDeadband = 0.05;
- if (::std::abs(throttle_) < kThrottleDeadband) {
- throttle_ = 0;
- } else {
- throttle_ = copysign((::std::abs(throttle_) - kThrottleDeadband) /
- (1.0 - kThrottleDeadband), throttle_);
- }
-
- double neg_inertia_scalar;
- if (highgear_) {
- neg_inertia_scalar = 8.0; // used to be csvReader->NEG_INTERTIA_HIGH
- sensitivity = 1.22; // used to be csvReader->SENSE_HIGH
- } else {
- if (wheel * neg_inertia > 0) {
- neg_inertia_scalar = 5; // used to be csvReader->NEG_INERTIA_LOW_MORE
- } else {
- if (::std::abs(wheel) > 0.65) {
- neg_inertia_scalar = 5; // used to be csvReader->NEG_INTERTIA_LOW_LESS_EXT
- } else {
- neg_inertia_scalar = 5; // used to be csvReader->NEG_INTERTIA_LOW_LESS
- }
- }
- sensitivity = 1.24; // used to be csvReader->SENSE_LOW
- }
- double neg_inertia_power = neg_inertia * neg_inertia_scalar;
- _neg_inertia_accumulator += neg_inertia_power;
-
- wheel = wheel + _neg_inertia_accumulator;
- if (_neg_inertia_accumulator > 1) {
- _neg_inertia_accumulator -= 1;
- } else if (_neg_inertia_accumulator < -1) {
- _neg_inertia_accumulator += 1;
- } else {
- _neg_inertia_accumulator = 0;
- }
-
- linear_power = throttle_;
-
- if (quickturn_) {
- double qt_angular_power = wheel;
- if (::std::abs(linear_power) < 0.2) {
- if (qt_angular_power > 1) qt_angular_power = 1.0;
- if (qt_angular_power < -1) qt_angular_power = -1.0;
- } else {
- qt_angular_power = 0.0;
- }
- overPower = 1.0;
- if (highgear_) {
- sensitivity = 1.0;
- } else {
- sensitivity = 1.0;
- }
- angular_power = wheel;
- } else {
- overPower = 0.0;
- angular_power = ::std::abs(throttle_) * wheel * sensitivity;
- }
-
- _right_pwm = _left_pwm = linear_power;
- _left_pwm += angular_power;
- _right_pwm -= angular_power;
-
- if (_left_pwm > 1.0) {
- _right_pwm -= overPower*(_left_pwm - 1.0);
- _left_pwm = 1.0;
- } else if (_right_pwm > 1.0) {
- _left_pwm -= overPower*(_right_pwm - 1.0);
- _right_pwm = 1.0;
- } else if (_left_pwm < -1.0) {
- _right_pwm += overPower*(-1.0 - _left_pwm);
- _left_pwm = -1.0;
- } else if (_right_pwm < -1.0) {
- _left_pwm += overPower*(-1.0 - _right_pwm);
- _right_pwm = -1.0;
- }
- }
-
- void SendMotors(Drivetrain::Output *output) {
- LOG(DEBUG, "left pwm: %f right pwm: %f wheel: %f throttle: %f\n",
- _left_pwm, _right_pwm, wheel_, throttle_);
- if (output) {
- output->left_voltage = _left_pwm * 12.0;
- output->right_voltage = _right_pwm * 12.0;
- }
- if (highgear_) {
- shifters.MakeWithBuilder().set(false).Send();
- } else {
- shifters.MakeWithBuilder().set(true).Send();
- }
- }
-
- private:
- double _old_wheel;
- double wheel_;
- double throttle_;
- bool quickturn_;
- bool highgear_;
- double _neg_inertia_accumulator;
- double _left_pwm;
- double _right_pwm;
-};
-
void DrivetrainLoop::RunIteration(const Drivetrain::Goal *goal,
const Drivetrain::Position *position,
Drivetrain::Output *output,
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 0f8c87e..0119dd2 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -11,7 +11,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "frc971/queues/GyroAngle.q.h"
+#include "frc971/queues/gyro_angle.q.h"
using ::aos::time::Time;
diff --git a/frc971/control_loops/python/wrists.py b/frc971/control_loops/python/claw.py
similarity index 63%
rename from frc971/control_loops/python/wrists.py
rename to frc971/control_loops/python/claw.py
index d752000..3d6b9fc 100755
--- a/frc971/control_loops/python/wrists.py
+++ b/frc971/control_loops/python/claw.py
@@ -5,9 +5,9 @@
import sys
from matplotlib import pylab
-class Wrist(control_loop.ControlLoop):
- def __init__(self, name="RawWrist"):
- super(Wrist, self).__init__(name)
+class Claw(control_loop.ControlLoop):
+ def __init__(self, name="RawClaw"):
+ super(Claw, self).__init__(name)
# Stall Torque in N m
self.stall_torque = 1.4
# Stall Current in Amps
@@ -16,7 +16,7 @@
self.free_speed = 6200.0
# Free Current in Amps
self.free_current = 1.5
- # Moment of inertia of the wrist in kg m^2
+ # Moment of inertia of the claw in kg m^2
# TODO(aschuh): Measure this in reality. It doesn't seem high enough.
# James measured 0.51, but that can't be right given what I am seeing.
self.J = 2.0
@@ -58,9 +58,9 @@
self.InitializeState()
-class WristDeltaU(Wrist):
- def __init__(self, name="Wrist"):
- super(WristDeltaU, self).__init__(name)
+class ClawDeltaU(Claw):
+ def __init__(self, name="Claw"):
+ super(ClawDeltaU, self).__init__(name)
A_unaugmented = self.A
B_unaugmented = self.B
@@ -97,58 +97,74 @@
self.InitializeState()
-def ClipDeltaU(wrist, delta_u):
- old_u = numpy.matrix([[wrist.X[2, 0]]])
- new_u = numpy.clip(old_u + delta_u, wrist.U_min, wrist.U_max)
+def ClipDeltaU(claw, delta_u):
+ old_u = numpy.matrix([[claw.X[2, 0]]])
+ new_u = numpy.clip(old_u + delta_u, claw.U_min, claw.U_max)
return new_u - old_u
def main(argv):
# Simulate the response of the system to a step input.
- wrist = WristDeltaU()
+ claw = ClawDeltaU()
simulated_x = []
for _ in xrange(100):
- wrist.Update(numpy.matrix([[12.0]]))
- simulated_x.append(wrist.X[0, 0])
+ claw.Update(numpy.matrix([[12.0]]))
+ simulated_x.append(claw.X[0, 0])
pylab.plot(range(100), simulated_x)
pylab.show()
# Simulate the closed loop response of the system to a step input.
- wrist = WristDeltaU()
+ top_claw = ClawDeltaU("TopClaw")
close_loop_x = []
close_loop_u = []
R = numpy.matrix([[1.0], [0.0], [0.0]])
- wrist.X[2, 0] = -5
+ top_claw.X[2, 0] = -5
for _ in xrange(100):
- U = numpy.clip(wrist.K * (R - wrist.X_hat), wrist.U_min, wrist.U_max)
- U = ClipDeltaU(wrist, U)
- wrist.UpdateObserver(U)
- wrist.Update(U)
- close_loop_x.append(wrist.X[0, 0] * 10)
- close_loop_u.append(wrist.X[2, 0])
+ U = numpy.clip(top_claw.K * (R - top_claw.X_hat), top_claw.U_min, top_claw.U_max)
+ U = ClipDeltaU(top_claw, U)
+ top_claw.UpdateObserver(U)
+ top_claw.Update(U)
+ close_loop_x.append(top_claw.X[0, 0] * 10)
+ close_loop_u.append(top_claw.X[2, 0])
pylab.plot(range(100), close_loop_x)
pylab.plot(range(100), close_loop_u)
pylab.show()
# Write the generated constants out to a file.
- if len(argv) != 5:
+ if len(argv) != 9:
print "Expected .h file name and .cc file name for"
print "both the plant and unaugmented plant"
else:
- unaug_wrist = Wrist("RawWrist")
- unaug_loop_writer = control_loop.ControlLoopWriter("RawWrist",
- [unaug_wrist])
- 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("Wrist", [wrist])
+ top_unaug_claw = Claw("RawTopClaw")
+ top_unaug_loop_writer = control_loop.ControlLoopWriter("RawTopClaw",
+ [top_unaug_claw])
if argv[1][-3:] == '.cc':
- loop_writer.Write(argv[2], argv[1])
+ top_unaug_loop_writer.Write(argv[2], argv[1])
else:
- loop_writer.Write(argv[1], argv[2])
+ top_unaug_loop_writer.Write(argv[1], argv[2])
+
+ top_loop_writer = control_loop.ControlLoopWriter("TopClaw", [top_claw])
+ if argv[3][-3:] == '.cc':
+ top_loop_writer.Write(argv[4], argv[3])
+ else:
+ top_loop_writer.Write(argv[3], argv[4])
+
+ bottom_claw = ClawDeltaU("BottomClaw")
+ bottom_unaug_claw = Claw("RawBottomClaw")
+ bottom_unaug_loop_writer = control_loop.ControlLoopWriter(
+ "RawBottomClaw", [bottom_unaug_claw])
+ if argv[5][-3:] == '.cc':
+ bottom_unaug_loop_writer.Write(argv[6], argv[5])
+ else:
+ bottom_unaug_loop_writer.Write(argv[5], argv[6])
+
+ bottom_loop_writer = control_loop.ControlLoopWriter("BottomClaw",
+ [bottom_claw])
+ if argv[7][-3:] == '.cc':
+ bottom_loop_writer.Write(argv[8], argv[7])
+ else:
+ bottom_loop_writer.Write(argv[7], argv[8])
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/update_claw.sh b/frc971/control_loops/update_claw.sh
new file mode 100755
index 0000000..083850a
--- /dev/null
+++ b/frc971/control_loops/update_claw.sh
@@ -0,0 +1,14 @@
+#!/bin/bash
+#
+# Updates the claw controllers.
+
+cd $(dirname $0)
+
+./python/claw.py claw/unaugmented_top_claw_motor_plant.h \
+ claw/unaugmented_top_claw_motor_plant.cc \
+ claw/top_claw_motor_plant.h \
+ claw/top_claw_motor_plant.cc \
+ claw/unaugmented_bottom_claw_motor_plant.h \
+ claw/unaugmented_bottom_claw_motor_plant.cc \
+ claw/bottom_claw_motor_plant.h \
+ claw/bottom_claw_motor_plant.cc
diff --git a/frc971/control_loops/update_wrists.sh b/frc971/control_loops/update_wrists.sh
deleted file mode 100755
index bb79a0a..0000000
--- a/frc971/control_loops/update_wrists.sh
+++ /dev/null
@@ -1,10 +0,0 @@
-#!/bin/bash
-#
-# Updates the wrist controllers.
-
-cd $(dirname $0)
-
-./python/wrists.py wrists/top_wrist_motor_plant.h \
- wrists/top_wrist_motor_plant.cc \
- wrists/bottom_wrist_motor_plant.h \
- wrists/bottom_wrist_motor_plant.cc
diff --git a/frc971/control_loops/wrists/bottom_wrist_motor_plant.h b/frc971/control_loops/wrists/bottom_wrist_motor_plant.h
deleted file mode 100644
index 00db927..0000000
--- a/frc971/control_loops/wrists/bottom_wrist_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_WRISTS_BOTTOM_WRIST_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_WRISTS_BOTTOM_WRIST_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeRawWristPlantCoefficients();
-
-StateFeedbackController<2, 1, 1> MakeRawWristController();
-
-StateFeedbackPlant<2, 1, 1> MakeRawWristPlant();
-
-StateFeedbackLoop<2, 1, 1> MakeRawWristLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_WRISTS_BOTTOM_WRIST_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/wrists/top_wrist_motor_plant.h b/frc971/control_loops/wrists/top_wrist_motor_plant.h
deleted file mode 100644
index ac657cb..0000000
--- a/frc971/control_loops/wrists/top_wrist_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_WRISTS_TOP_WRIST_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_WRISTS_TOP_WRIST_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<3, 1, 1> MakeWristPlantCoefficients();
-
-StateFeedbackController<3, 1, 1> MakeWristController();
-
-StateFeedbackPlant<3, 1, 1> MakeWristPlant();
-
-StateFeedbackLoop<3, 1, 1> MakeWristLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_WRISTS_TOP_WRIST_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/wrists/wrists.cc b/frc971/control_loops/wrists/wrists.cc
deleted file mode 100644
index fe9295b..0000000
--- a/frc971/control_loops/wrists/wrists.cc
+++ /dev/null
@@ -1,80 +0,0 @@
-#include "frc971/control_loops/wrists/wrists.h"
-
-#include <stdio.h>
-
-#include <algorithm>
-
-#include "aos/common/control_loop/control_loops.q.h"
-#include "aos/common/logging/logging.h"
-
-#include "frc971/constants.h"
-#include "frc971/control_loops/wrists/top_wrist_motor_plant.h"
-#include "frc971/control_loops/wrists/bottom_wrist_motor_plant.h"
-
-namespace frc971 {
-namespace control_loops {
-
-WristMotor::WristMotor(control_loops::WristLoop *my_wrist)
- : aos::control_loops::ControlLoop<control_loops::WristLoop>(my_wrist),
- zeroed_joint_(MakeWristLoop()) {
- {
- using ::frc971::constants::GetValues;
- ZeroedJoint<1>::ConfigurationData config_data;
-
- config_data.lower_limit = GetValues().wrist_lower_limit;
- config_data.upper_limit = GetValues().wrist_upper_limit;
- config_data.hall_effect_start_angle[0] =
- GetValues().wrist_hall_effect_start_angle;
- config_data.zeroing_off_speed = GetValues().wrist_zeroing_off_speed;
- config_data.zeroing_speed = GetValues().wrist_zeroing_speed;
-
- config_data.max_zeroing_voltage = 5.0;
- config_data.deadband_voltage = 0.0;
-
- zeroed_joint_.set_config_data(config_data);
- }
-}
-
-// Positive angle is up, and positive power is up.
-void WristMotor::RunIteration(
- const ::aos::control_loops::Goal *goal,
- const control_loops::WristLoop::Position *position,
- ::aos::control_loops::Output *output,
- ::aos::control_loops::Status * status) {
-
- // Disable the motors now so that all early returns will return with the
- // motors disabled.
- if (output) {
- output->voltage = 0;
- }
-
- ZeroedJoint<1>::PositionData transformed_position;
- ZeroedJoint<1>::PositionData *transformed_position_ptr =
- &transformed_position;
- 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;
- }
-
- const double voltage = zeroed_joint_.Update(transformed_position_ptr,
- output != NULL,
- goal->goal, 0.0);
-
- if (position) {
- LOG(DEBUG, "pos: %f hall: %s absolute: %f\n",
- position->pos,
- position->hall_effect ? "true" : "false",
- zeroed_joint_.absolute_position());
- }
-
- if (output) {
- output->voltage = voltage;
- }
- status->done = ::std::abs(zeroed_joint_.absolute_position() - goal->goal) < 0.004;
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/frc971/control_loops/wrists/wrists.h b/frc971/control_loops/wrists/wrists.h
deleted file mode 100644
index 1c1fdf4..0000000
--- a/frc971/control_loops/wrists/wrists.h
+++ /dev/null
@@ -1,63 +0,0 @@
-#ifndef FRC971_CONTROL_LOOPS_WRIST_WRIST_H_
-#define FRC971_CONTROL_LOOPS_WRIST_WRIST_H_
-
-#include <memory>
-
-#include "aos/common/control_loop/ControlLoop.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/control_loops/wrists/wrists.q.h"
-#include "frc971/control_loops/wrists/top_wrist_motor_plant.h"
-#include "frc971/control_loops/wrists/bottom_wrist_motor_plant.h"
-
-#include "frc971/control_loops/zeroed_joint.h"
-
-namespace frc971 {
-namespace control_loops {
-namespace testing {
-class WristTest_NoWindupPositive_Test;
-class WristTest_NoWindupNegative_Test;
-};
-
-class WristMotor
- : public aos::control_loops::ControlLoop<control_loops::WristLoop> {
- public:
- explicit WristMotor(
- control_loops::WristLoop *my_wrist = &control_loops::wrist);
-
- // True if the goal was moved to avoid goal windup.
- bool capped_goal() const { return zeroed_joint_.capped_goal(); }
-
- // True if the wrist is zeroing.
- bool is_zeroing() const { return zeroed_joint_.is_zeroing(); }
-
- // True if the wrist 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 ::aos::control_loops::Goal *goal,
- const control_loops::WristLoop::Position *position,
- ::aos::control_loops::Output *output,
- ::aos::control_loops::Status *status);
-
- private:
- // Friend the test classes for acces to the internal state.
- friend class testing::WristTest_NoWindupPositive_Test;
- friend class testing::WristTest_NoWindupNegative_Test;
-
- // The zeroed joint to use.
- ZeroedJoint<1> zeroed_joint_;
-
- DISALLOW_COPY_AND_ASSIGN(WristMotor);
-};
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // FRC971_CONTROL_LOOPS_WRIST_WRIST_H_
diff --git a/frc971/control_loops/wrists/wrists.q b/frc971/control_loops/wrists/wrists.q
deleted file mode 100644
index 36159c9..0000000
--- a/frc971/control_loops/wrists/wrists.q
+++ /dev/null
@@ -1,28 +0,0 @@
-package frc971.control_loops;
-
-import "aos/common/control_loop/control_loops.q";
-
-// All angles here are 0 horizontal, positive up.
-queue_group WristsLoop {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- // The angle of the bottom wrist.
- double bottom_angle;
- // How much higher the top wrist is.
- double between_angle;
- bool intake;
- };
- message Position {
- double bottom_position, top_position;
- bool bottom_hall_effect, top_hall_effect;
- double bottom_calibration, top_calibration;
- };
-
- queue Goal goal;
- queue Position position;
- queue aos.control_loops.Output output;
- queue aos.control_loops.Status status;
-};
-
-queue_group WristsLoop wrists;
diff --git a/frc971/control_loops/wrists/wrists_main.cc b/frc971/control_loops/wrists/wrists_main.cc
deleted file mode 100644
index 66d1ea3..0000000
--- a/frc971/control_loops/wrists/wrists_main.cc
+++ /dev/null
@@ -1,11 +0,0 @@
-#include "frc971/control_loops/wrist/wrists.h"
-
-#include "aos/linux_code/init.h"
-
-int main() {
- ::aos::Init();
- frc971::control_loops::WristsLoop wrists;
- wrists.Run();
- ::aos::Cleanup();
- return 0;
-}
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index d841672..d345b0c 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/claw/claw.gyp:claw',
+ '../control_loops/claw/claw.gyp:claw_lib_test',
'../autonomous/autonomous.gyp:auto',
'../input/input.gyp:joystick_reader',
'../output/output.gyp:motor_writer',